Pick & Place

Autor: Daniel Block
Betreuer: Prof. Schneider

www.lego.de

Motivation

Ein Roboterarm ist ein tolles Anschauungsobjekt, um etwas über Koordinatentransformation zu lernen.

Ziel

Programmieren Sie einen Roboterarm so, dass er sich ein Zuckerstück von einem Haufen nimmt und dieses in eine HSHL-Tasse fallen lässt.

Aufgabe

1. Erkennen Sie mit einer Webcam einen Zuckerwürfel.
2. Konstruieren Sie den Standard Lego Roboterarm so, dass er den Zuckerwürfel greifen kann.
3. Analysieren Sie die Videoszene und steuern Sie den Roboterarm, so dass er den Zucker Greift und diesen kollisionsfrei in die Tasse fallen lässt.

Vorbereitungen

Vor der Bearbeitung der Aufgabe sind einige Vorbereitungen zu treffen. Zum einen wird ein Grundverständnis für die Bildverarbeitung und die Koordinatentransformation vorausgesetzt. Dieses wurde im Rahmen der Veranstaltung Digitale Signal- und Bildverarbeitung während des Sommersemesters 2014 im Studiengang Mechatronik der HSHL erlangt. Eine weitere Recherche in Literatur und im Internet erwies sich jedoch als sehr zielführend. Besonders zur Umsetzung in MATLAB konnten hier einige hilfreiche Hinweise gefunden werden. Zum anderen sind einige Hardware- und Softwarekomponenten erforderlich. Diese werden im Folgenden kurz dargestellt.

Erforderliches Equipment

• Lego NXT Roboter
  • Lego Mindstorms Basisset 9797
  • Lego Mindstorms Ergänzungsset 9648/9695
• Webcam (z. B. Logitech C310)
• HSHL-Tasse
• Zuckerstücke
• Tesafilm
• Notebook oder Rechner mit MATLAB

Erforderliche Software

• MATLAB (es wird die Version 2014a empfohlen aufgrund der Image Processing Toolbox)
• RWTH - Mindstorms NXT Toolbox für MATLAB
• LEGO Mindstorms NXT Treiber Fantom
• NXT Firmware Version 1.28 oder höher
• NeXTTool
• Camera Calibration Toolbox für MATLAB

Die Hyperlinks führen jeweils zur Download-Seite. MATLAB ist über das ZfW (Zentrum für Wissensmanagement) der HSHL zu bekommen.

Installation der Treiber

Diese Schritt-für-Schritt-Anleitung orientiert sich an der Beschreibung der RTWH Aachen .

1. Toolbox der RWTH Aachen
   1. Toolbox der RWTH Aachen herunterladen.
   2. Heruntergeladene Datei extrahieren.
   3. Toolbox als MATLAB-Pfad hinzufügen.
      1. Set Path in MATLAB aufrufen.
      2. Add with subfolders aufrufen und Verzeichnis der Toolbox auswählen.
      3. Speichern nicht vergessen.
2. Firmware des NXT
   1. Version der Firmware auf dem NXT überprüfen. Die Version lässt sich im Menü des NXT einsehen.
   2. Wenn die Version älter ist als 1.28 bzw. 1.29, die Firmware herunterladen.
   3. Die Firmware auf den NXT laden, z. B. mit der Funktion FlashNXTWithFirmware1.29.bat in /tools/MotorControl im Verzeichnis der Toolbox.
3. Laden von MotorControl auf NXT
   1. NeXTool herunterladen und im Verzeichnis /tools/MotorControl der Toolbox speichern.
   2. In Windows-Systemen nun die Funktion TransferMotorControlBinaryToNXT.bat aufrufen, um mit NeXTool die MotorControl*.rxe auf den NXT zu laden.
   3. Den Schritten im Command-Fenster folgen.
4. Treiber Wird beispielhaft für Windows 32 bit mit USB-Verbindung erklärt. Das Vorgehen für andere Betriebssysteme oder die Bluetooth-Verbindung kann auf der Internetseite der RWTH Aachen nachgesehen werden unter Check other software requirements.
   1. NXT Fantom-Treiber herunterladen und installieren.
   2. Treiber auf den NXT laden mit TransferMotorControlBinaryToNXT
5. Funktionstext
   1. USB-Verbindung zwischen NXT und Rechner bzw. Notebook herstellen.
   2. NXT einschalten.
   3. MATLAB starten.
   4. Im Command Window von MATLAB COM_OpenNXT ausführen (einfach eintippen und mit Enter ausführen).
   5. Wird der Aufruf ohne Fehler ausgeführt, ist die Installation erfolgreich abgeschlossen.

Installation der Camera Calibration Toolbox

Diese Beschreibung ist angelehnt an die Erläuterungen von Jean-Yves Bouguet .

1. Toolbox herunterladen.
2. Heruntergeladene Datei extrahieren.
3. Den Ordner TOOLBOX_calib als MATLAB-Pfad hinzufügen (Vorgehen vgl. RWTH-Toolbox s. o.).
4. Starten mit ausführen von calib_gui in MATLAB.

Die genaue Vorgehensweise bei der Kalibrierung der Kamera wird an dieser Stelle nicht erläutert. Ich verweise an dieser Stelle auf die Homepage zur Toolbox und den Artikel von Herrn Deitel zu diesem Thema .

Hardware

Der Aufbau des Roboters erfolgt mit den Komponenten der Lego Mindstorms Kästen. Die Teile entsprechen dem Lego Technik Sortiment. Die wesentlichen Komponenten sind der intelligente Stein Brick, drei Motoren und die Webcam. Desweiteren werden diverse Konstruktions-, Verbindungs- und Getriebe-Teile benötigt. Da mit dem NXT nur drei Motoren betrieben werden können, wurde die Bewegung des Roboters auf folgende drei Dimensionen festgelegt:

• Drehen des Roboterarms auf dem Unterbau
• Heben und Senken des Greifers
• Öffnen und Schließen des Greifers

Der drehbare Arm wird von einem Drehkranz getragen, der von einem Motor angetrieben wird. Der Motor für das Heben und Senken ist zugleich tragendes Element des Armes auf dem Drehkranz. Der Greifer ist so konstruiert, dass ein Schenkel fest montiert ist und der zweite von einem weiteren Motor angetrieben wird. Für das Öffnen des Greifers und für das Drehen des Armes werden mehrstufige Übersetzungs-Getriebe eingesetzt. Dies ermöglicht langsame Bewegungen bei zugleich hohem Drehmoment. Der intelligente Stein ist am Rahmen montiert und bildet somit einen Teil des Gegengewichtes für den Roboterarm. Der Aufbau im Einzelnen ist in den Fotos zu sehen, die im Projekt-Ordner im SVN gespeichert sind. Desweiteren ist dort ein Lego-Designer-Modell des Roboters hinterlegt, dass dem Aufbau sehr nahe kommt. Die Abweichungen ergeben sich durch Verwendung etwas anderer Teile oder die Montage von Zahnrädern in den genannten Getrieben, die im Modell nicht möglich ist. Desweiteren ist ein Verbindungsstein im Modell nicht integriert, weil die Lage vom Lego-Standard abweicht. Diese Teile können aber in den Fotos des Roboters gut wiedererkannt werden. Der Vollständigkeit halber sind diese Teile im Lego-Designer-Modell als lose Teile beigefügt.

Software

Im Folgenden wird die Implementierung in MATLAB zur Erkennung der Zuckerstücke und der Tasse sowie die Ansteuerung der Motoren dargestellt.

Bilderkennung

Zu aller Erst wird die Kamera initialisert. Dazu wird eine Objekt-Variable erstellt, auf die im Programmverlauf zugegriffen werden kann.

% Kamera-Verbindung
cam = webcam(1);

Vor dem Beginn der Bilderkennung wird der Roboterarm an die linke Grenze des Arbeitsbereiches gedreht. Dazu wird er angehoben, um nicht mit Gegenständen zu kollidieren und der Greifer geöffnet. Die Suche der Zuckerstücke erfolgt in einer while-Schleife, die bei gefundenem Zuckerstück abgebrochen wird. Von der zuvor angefahrenen Position beginnt der Roboter den Arm langsam nach rechts zu drehen. Währenddessen wird zyklisch die Kamera abgefragt und das aktuelle Bild in einer Variablen gespeichert.

%Bild einlesen 
img=snapshot(cam);

Zur Minimierung der Daten wird nun ein Graustufenbild erzeugt.

%in Graustufen umwandeln
img_gray=rgb2gray(img);

Der Kontrast des Graustufenbildes wird erhöht durch eine einfache Justierung und durch die adaptive Histrogrammlinearisierung.

%Kontrast erhöhen
img_contrast=imadjust(img_gray);
% mit Adaptiver Histogrammlinearisierung
img_adapth_contrast=adapthisteq(img_contrast);

Das so vorbereite Bild wird als Grundlage für die Kantenerkennung mit dem Sobel-Algorithmus verwendet.

% Kantenerkennung mit Sobel
img_sobel=edge(img_adapth_contrast,'sobel');

Für das Kantenbild wird nun ein Labeling durchgeführt, sodass zusammenhängende Regionen erfasst werden. Hierfür bietet MATLAB die Funktion regionprobs, was für region properties steht. Die gefunden Regionen werden nun nach ihrer Lage und Größe gefiltert. Es werden nur Regionen mit der Größe 30 bis 100 Pixel berücksichtigt. Desweiteren wird das obere Drittel sowie der linke und der rechte äußerste Rand abgeschnitten. Diese Filterung wurde im Test als sehr zielführend und zuverlässig erfunden.

size_i=size(img_sobel);
regions=regionprops(img_sobel);
k=1;
% Filtern nach Größe und Lage
for m=1:length(regions)
    if regions(m).Area>30 && regions(m).Area<100 && regions(m).BoundingBox(2)>(0.3*size_i(1)) && regions(m).BoundingBox(1)>10 && regions(m).BoundingBox(1)<620
        regions_filt(k)=regions(m);
        k=k+1;
    end
end

Um Fehler auszuschließen wird darauf die Anzahl der gefundenen Regionen untersucht. Liegt diese im Bereich von 3 bis 19, werden die Punktpaare der Mittelpunkte der Regionen nach ihrer Höhe sortiert. Somit kann das oberste Zuckerstück des Haufens lokalisiert werden. An dieser Stelle ist die Suche nach Zuckerstücken beendet. Die Lokalisierung der Tasse erfolgt nach dem gleichen Verfahren. Es werden jedoch nur Regionen berücksichtigt, die größer sind als 150 Pixel. Die Überprüfung der Anzahl wird mit dem Wert 5 als Obergrenze durchgeführt.

Hardware-Ansteuerung

Bevor der NXT angesteuert werden kann, ist eine Initialisierung einer entsprechenden Objekt-Variablen notwendig.

% Verbindung aufbauen und Globalsetzung NXT
handle = COM_OpenNXT();
COM_SetDefaultNXT(handle);

Das Ansteuern der Motoren geschieht immer nach dem gleichen Prinzip. Es werden die Geschwindigkeit, die Länge der Bewegung in Grad der Motorumdrehung und einge Parameter de finiert und auf den NXT gesendet. Es wird nun ein Beispiel gezeigt.

% Anlegen des Motor-Objektes; Definition des Ports (hier B) und der Geschwindigkeit, das Vorzeichen bestimmt die Richtung
motorB = NXTMotor('B', 'Power', -50);%>0 = runter, <0 = hoch
% Festlegen der Bewegungslänge, hier 60 Grad
motorB.TachoLimit = 60; %60 passt für Höhe der HSHL-Tasse
% Option, was nach Erreichen der Grenze geschehen soll: 'Holdbrake' - Position halten, 'Coast' - sanft abbremsen
motorB.ActionAtTachoLimit = 'Holdbrake';
% Sanfter Start, verhindert Bornout, führt zu Abweichungen
motorB.SmoothStart = false;
% Ein-/Ausschalten der geregelten Geschwindigkeitssteuerung über ein dynamisches Drehmoment
motorB.SpeedRegulation = false;
% Senden der Daten
motorB.SendToNXT();
% Program bis zur Beendigung der Motoransteuerung anhalten
motorB.WaitFor();
% Stoppen des Motors
motorB.Stop('off');

Desweiteren gibt es die Möglichkeit die aktuelle Motor-Position als Nullpunkt festzulegen. Dies ist besonders hilfreich, um an definierte Stellen zurückzukehren.

motorA.ResetPosition();
%Motorposition soll bei 0 beginnen

Außerdem können Motordaten wie die aktuelle Position ausgelesen werden.

dataB = motorB.ReadFromNXT();
%auslesen der Motordaten des NXT

Mit Hilfe dieser Befehle können sämtliche Bewegungen realisiert werden.
Die Parameter wurden in einigen Versuchen an den Aufbau angepasst. Dabei spielen zum Beispiel verwendete Übersetzungen und die erforderliche Kraft eine entscheidende Rolle. Der Drehantrieb wurde mit einer zweistufigen Übersetzung mit dem Verhältnis 1:2.8 realisiert.
Die Koordinatentransformation konnte vereinfacht werden, da die Länge des Roboterarmes fest ist. Sie erstreckt sich somit nur auf die Rotation um einen entsprechenden Winkel. Dieser ergibt sich aus der Summe der Bildkoordinate multipliziert mit der Entsprechung eines Pixels in Weltkoordinaten und der Verschiebung von Kamera zu Greifer. Für diesen Roboter wurde für die Pixel festgestellt, dass eine Pixelbreite 0.05 Grad in Weltkoordinaten entsprechen. Die Verschiebung wurde mit 20 Grad ermittelt. Bei der Motoransteuerung darf die Übersetzung nicht vergessen werden.

Video

Das Video des Funktionsnachweises findet sich auf YouTube.

Reflexion - Lessons Learned

Es konnten einige Verfahren und Algorithmen der Bildverarbeitung angewendet werden. Jedoch zeigte sich die fehlende Präzision des Lego-Systems. Die Aufgabe konnte aber zufriedenstellend gelöst werden.

Siehe auch

• Objekterkennung mit Kamera
• Camera Calibration Toolbox
• RWTH Aachen Toolbox meets NXT
• Lego Mindstorms
• Tönnies, Klaus D.: Grundlagen der Bildverabeitung. Pearson Studium, München, 2005. ISBN 3-8273-7155-4

Weblinks

• Beschreibung, Tutorial und Downlod der Camera Calibration Toolbox für MATLAB von Jean-Yves Bouguet
• Beschreibung der Klasse NXTmotor der RWTH Aachen
• MATLAB-Seite zur Image Processing Toolbox
• Homepage der RWTH Aachen zur Mindstorms NXT Toolbox für MATLAB
• Downloadseite von Lego

---

→ zurück zum Hauptartikel: Digitale Signal- und Bildverarbeitung SoSe2014