Dynamische Regelung eines Industrieroboters in Echtzeit

Dynamische Regelung eines
Industrieroboters in Echtzeit
von J. Gut und H. Rapp
Lernziele
1. Systemanalyse eines Industrieroboters
2. Implementierung eines physikalisch modellierten diskreten Reglers
3. Test und Bewertung verschiedener Parameter des Regelkreises
F–1
Inhaltsverzeichnis
1
Einleitung
4
2
Versuchsaufbau und Einordnung der Aufgabe
4
3
Einführung in Python und Linux
6
3.1
Linux . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
3.2
Python Editor und Konsole . . . . . . . . . . . . . . . . . . . . . . . . . .
7
4
5
Systemanalyse des Industrieroboters
7
4.1
Einführung in die Robotersteuerung . . . . . . . . . . . . . . . . . . . . .
7
4.2
Bestimmung der Verzögerungszeit T0 des Robotersystems . . . . . . . . .
8
4.2.1
Aufgabe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8
4.2.2
Fragen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10
Regelung des Industrieroboters
12
5.1
Jonglieren eines Tischtennisballs . . . . . . . . . . . . . . . . . . . . . . .
13
5.1.1
Aufgabe (schriftlich vorzubereiten) . . . . . . . . . . . . . . . . .
14
Parameter eines PID Reglers . . . . . . . . . . . . . . . . . . . . . . . . .
16
5.2.1
Aufgabe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
5.2.2
Fragen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
5.2
A Programmieren mit Python
19
A.1 Datentypen und Operatoren . . . . . . . . . . . . . . . . . . . . . . . . . .
19
A.2 Bibliotheken und Hilfe . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
A.2.1 Bibliotheken . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
A.2.2 Python Hilfe . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
A.3 Ein- und Ausgabe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
A.4 Beispiele für Verzweigungen und Schleifen . . . . . . . . . . . . . . . . .
21
A.5 Funktionen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
22
A.6 Vektoren und Matrizen . . . . . . . . . . . . . . . . . . . . . . . . . . . .
22
F–2
Hinweise
• Der Versuchsaufbau darf nur unter Aufsicht des Betreuers in Betrieb genommen werden.
• Der Roboter darf ausschließlich vom Betreuer bedient werden.
• Das Betreten der Schutzumzäunung ist gefährlich und deswegen verboten.
• Die Aufgabe in Abschnitt 5.1.1 ist schriftlich vorzubereiten.
F–3
1
Einleitung
Industrieroboter sind die Alleskönner der Automatisierungstechnik. Bisher nur sehr großen
Produktionsanlagen mit sehr hohen Stückzahlen vorbehalten, finden Sie in zunehmendem
Maße auch Anwendung in Kleinserien. Ein besonderer Trend der letzen Zeit ist die steigende
Interaktivität der Roboter die inzwischen auch an kooperativen Arbeitsplätzen direkt neben
dem Menschen eingesetzt werden können. Sorgfältig übereinander gestapelte Bauteile, etwa der Kotflügel einer Autokarosserie, sind für die Montage mit heutigen Industrierobotern
kein Problem. Schwieriger gestaltet sich beispielsweise die Entnahme von Bremsklötzen aus
einem Container. Dieses Problem ist in der Fachwelt auch als ,,Griff in die Kiste” bekannt.
Um dies zu leisten müssen die Roboter mehr von ihrer Umwelt sehen und verstehen lernen,
als es in der Vergangenheit der Fall war.
Dieser Versuch untersucht die kognitive Leistungsfähigkeit heutiger Systeme exemplarisch
an einem kleinen Industrieroboter. Wie auch der Mensch bezieht der Roboter seine Informationen über die Umwelt durch visuelle Wahrnehmung. Die Aufgabe des Roboters besteht
darin einen Tischtennisball autonom zu jonglieren. Der Ping-Pong-Roboter muss hierbei
aber nicht nur Objekte (Tischtennisball) in Bildern erkennen, sondern auch deren Verhalten
vorhersagen, interpretieren und eine eigene Reaktion ableiten. Der Versuch umfasst neben
der Systemanalyse, in der das Reaktionsvermögen des Roboters ermittelt werden soll, die
Berechnung eines deterministischen Verhaltens des Roboters sowie die Regelung des Tischtennisballs in einen stationären Zustand.
2
Versuchsaufbau und Einordnung der Aufgabe
Der Versuchsaufbau besteht aus zwei zueinander um 90◦ versetzten Kameras (Sensorik), einem Kuka KR5 Sixx Roboter (Aktuator) und einem Rechner auf dem die Signalverarbeitung
und Regelung stattfindet. Der Aufbau ist in Abbildung 1 dargestellt.
Abbildung 2 zeigt eine vereinfachte Version des geschlossenen Regelkreises, die den Fluss
der Information klar darlegt. Die Details der Regelung werden später genau beschrieben.
Die momentane Position des Ping-Pong Schlägers wird in einem kartesischen Koordinatensystem (siehe Abschnitt 4.2) angegeben; diese Koordinaten werden als Zustandsgrößen
des Roboters betrachtet. Der Zustand des Roboters (und damit des Schlägers) wird durch
die Position r = (x, y, z)T und die Neigungswinkel ϕ = (A, B, C)T der Schlägerebene
beschrieben.
Durch seine Bewegung wirkt der Roboter auf den Ball und verändert damit die Ballflugbahn,
die durch eine dreidimensionale Trajektorie b(t) = (xb (t), yb (t), zb (t))T beschrieben wird.
Die momentane Lage des Balls wird durch die zwei Kameras und das dazugehörige Bildverarbeitungssystem gemessen. Mehrere dieser Messungen zu verschiedenen Zeitpunkten
können dann durch den Prädiktor in eine Schätzung der Auftreffparameter verrechnet werden. Die Auftreffparameter bestehen aus der dreidimensionalen Auftreffposition r̂ i , der dreidimensionalen Auftreffgeschwindigkeit v̂ i und dem Auftreffzeitpunkt t̂i . i steht hier für Impact. Die Regelung geschieht in diesem Versuch durch drei unabhängige Regler, die später im
Detail diskutiert und zum Teil implementiert werden müssen; als Eingabe verarbeiten diese
F–4
Abbildung 1: Versuchsaufbau
ri,soll
Regelung
rs , ϕs
Sample /
Hold
r̂i , v̂i , t̂i
Tracker /
Prädiktor
b̂
Bildverarbeitung
rs , ϕs
b
Roboter
r, ϕ
Physik des Balles
Abbildung 2: Informationsfluss
Regler die Schätzungen aus dem Prädiktor und verschiedene Parameter für das gewünschte
Verhalten des Roboters wie zum Beispiel die Sollauftreffposition des Balls auf den Schläger
r i,soll . Das Sample / Hold Glied sorgt dafür, dass der Roboter nicht zu viele Stellsignale bekommt, da er diese nicht alle abfahren kann. Erst wenn sich die Eingangsgrößen nicht mehr
zu stark ändern, gibt das Sample / Hold (Halte-)Glied seinen momentanen Eingang an den
Roboter weiter.
Die Software, welche die Signalverarbeitung und Regelung umfasst, muss also verschiedene
Aufgaben erfüllen:
1. Detektion des Balls in den von dem Kamerasystem gelieferten Bildern (Bildverarbeitung)
2. Berechnung der Ballposition aus den Kamerabildern über Triangulation (Bildverarbeitung)
3. Schätzung der Geschwindigkeiten und Beschleunigungen des Balls (Tracker)
F–5
4. Vorhersage von Auftreffpunkt, Auftreffgeschwindigkeit und Auftreffzeitpunkt des
Balls (Prädiktor)
5. Ausrichtung und Positionierung des Tischtennisschlägers (Regler)
6. Regelung des Auftreffpunkts des Balls in einen gute Position für den Roboter; d.h. in
einen stationären Zustand des Gesamtsystems. (Regler)
Da ein Bearbeiten aller Aufgaben den zeitlichen Rahmen des Versuchs weit überschreiten
würde, wird nur die Regelungskomponente genauer betrachtet.
Der erste Schritt des Praktikumsversuchs besteht darin, eine Systemanalyse durchzuführen
und sich mit dem Robotersystem vertraut zu machen. Für das weitere Vorgehen wird hier
auch die Frage geklärt, mit welcher Verzögerungszeit der Roboter nach einem Stellbefehl
seine maximale Verfahrgeschwindigkeit erreicht hat (Abschnitt 4.2).
Im Anschluss wird dem Roboter das Jonglieren des Tischtennisballs beigebracht. Hierbei
wird ein Regler entworfen, der aus der Auftreffgeschwindigkeit die Stellwinkel errechnet,
die nötig sind um den Ball senkrecht nach oben zu spielen (Abschnitt 5.1).
Um dem System ausreichend Stabilität zu verleihen, wird zum Schluss eine Regelung ermittelt, welche den Tischtennisball in einen stationären Zustand regelt. Dies wird durch einen
weiteren Regler realisiert der die Stellwinkel so verändert, dass der Ball langsam in die Mitte
des Arbeitsbereiches des Roboters geregelt wird (Abschnitt 5.2).
3
Einführung in Python und Linux
Die Programmierung des Roboters erfolgt in der Programmiersprache Python. Python ist eine Multiparadigmensprache mit dem Ziel möglichst einfach und übersichtlich zu sein. Diese Eigenschaft ermöglicht besonders Programmieranfängern einen schnellen Einstieg. Für
das wissenschaftliche Arbeiten mit Python kann auf ein breites Spektrum an Bibliotheken
zurückgegriffen werden, die alle wichtigen Funktionen, wie das Arbeiten mit Matrizen und
Grafiken beinhalten. Die erstellten Programme werden auf einem Linux PC ausgeführt. Die
Kommunikation mit dem Roboter erfolgt via Ethernet. Die für das Bedienen und Starten der
Programme notwendigen Befehle werden im Folgenden kurz vorgestellt.
3.1
Linux
Für das Navigieren im Dateisystem und Starten von Programmen bietet es sich an das
Linux Terminal zu verwenden. Dieses kann über Anwendungen -> Zubehör ->
Terminal gestartet werden. Für das Arbeiten im Terminal werden folgende Befehle
benötigt:
• pwd - Zeigt das aktuelle Verzeichnis an
• ls - Gibt eine Liste aller Dateien und Ordner in einem Verzeichnis
F–6
• cd <Pfad> - Wechsel in einen Ordner oder in ein Verzeichnis
• cd .. - Wechselt in das darüberliegende Verzeichnis
• cd - Wechselt in das Home Verzeichnis
• ./pprobot.py - Starten des Python Programms pprobot.py
• idle player.py - Öffnet die Python Datei player.py mit dem Editor IDLE
Wichtig: Um ein Programm zu starten bzw. eine Datei zu öffnen, muss zuerst in den Ordner
gewechselt werden, in dem die Datei gespeichert ist.
3.2
Python Editor und Konsole
Für das Erstellen und Bearbeiten von Programmcode in Python gibt es eine große Auswahl
an Editoren. Prinzipiell reicht aber schon ein einfacher Texteditor wie er unter Windows und
Linux verfügbar ist um Programmcode zu schreiben oder zu verändern. Da diese Editoren
aber sehr unkomfortabel sind wird in diesem Versuch der Editor IDLE verwendet. Der Editor
kann über Klicken auf Anwendungen -> Entwicklung -> IDLE gestartet werden.
Es öffnen sich nun eine Konsole (Python Shell) und ein Editor. Im Editor kann über File
-> Open die zu bearbeitende Python Datei geöffnet werden. Das Öffnen der Datei mit dem
Editor IDLE über das Linux Terminal ist mit dem Befehl idle player.py ebenfalls
möglich. Weitere Informationen zu Python können dem Anhang entnommen werden.
4
4.1
Systemanalyse des Industrieroboters
Einführung in die Robotersteuerung
Bei dem in diesem Versuch verwendeten Roboter handelt es sich um einen VertikalKnickarmroboter. Dieser ist besonders in der Fertigungstechnik weit verbreitet. Er verfügt
insgesamt über sechs Achsen die sich aus drei Hauptachsen und drei Kopfachsen zusammensetzen. Der Arbeitsraum des Vertikal-Knickarmroboters ist hohlkugelförmig. Für die
Steuerung von Industrierobotern haben sich im wesentlichen drei Methoden etabliert, die
sich im Bewegungsverhalten und in der Art der Bewegungsprogrammierung unterscheiden.
Punkt-zu-Punkt-Steuerung (PTP, Point To Point): Bei der Punkt-zu-Punkt-Steuerung
werden durch die Programmierung Start- und Zielpunkte vorgegeben die der Roboter
anfahren soll. Jede Achse führt hierbei die zum Erreichen des Zielpunktes notwendige
Drehbewegung unabhängig voneinander aus. Die PTP-Steuerung ist nur dann geeignet, wenn zwischen zwei Punkten keine genaue Bahnkontur gefordert ist. Sie ist die
schnellste der Bewegungsmethoden und üblicherweise ruckoptimal, d.h. die mechanischen Teile des Roboters werden minimal belastet. Eingesetzt wird die PTP-Steuerung
z.B. beim Punktschweißen, Palettieren und Bohren.
F–7
Vielpunkt-Steuerung (MP, Multi Point): Bei der MP-Steuerung wird eine große Anzahl
an Punkten die auf einer Bahn liegen vorgegeben. Diese Steuerungsart wird vor allem beim ’Playback’ verwendet. ’Playback’ ist die Programmierung eines Roboters
über einen Handgriff, welcher wie ein Werkzeug am Roboter angebracht ist, und mit
dem man den Roboter bewegen kann. Die Programmiereinheit speichert hierbei mit
einer festen Taktrate die Position des Roboterarms und kann im Anschluss die vorgegebene Bahn nachfahren. Eine Programmierung über das Teach-In-Verfahren, bei der
verschiedene Punkte angefahren und gespeichert werden, ist ebenfalls möglich. Bei
der MP-Steuerung ist eine präzise Bahn und Geschwindigkeitssteuerung möglich. Sie
findet große Anwendung in der Automatisierungstechnik.
Bahnsteuerung (CP, Continuous Path): Die Bewegung des Roboters zwischen verschiedenen Punkten muss durch den Programmierer mit Hilfe von mathematischen Funktionen wie Geraden, Kreisen oder Polynomen festgelegt werden. Da nicht jede beliebige
Funktion durch den Roboter mechanisch ausgeführt werden kann, sind Fehler beim
Abfahren des Weges möglich.
In modernen Robotersystemen findet oft eine Vermischung der verschiedenen Steuerungsarten statt. Es existieren daher verschiedene kommerzielle Steuerungssysteme die auf Linear-,
Zirkular- und Spline-Interpolation basieren. Für diese Steuerungsarten genügt die Angabe
weniger Punkte und die Angabe einer Interpolationsart, um einen linearen oder zirkularen
Verlauf der Bahnkurve zu erreichen.
Um dem Roboter bestimme Zielpunkte Vorgeben zu können, müssen diese in einem zuvor
festgelegten Koordinatensystem liegen. Um für den Benutzer eine möglichst benutzerfreundliche Steuerung zu ermöglichen ist eine Eingabe der Zielpunkte für den Roboter in kartesischen Koordinaten möglich. Die Transformation der Koordinaten erfolgt durch die Software
auf dem externen Rechner und dem Robotersteuerungssystem. Das Roboterkoordinatensystem verwendet zum Beschreiben der Punkte die Winkelstellung der verschiedenen Achsen.
4.2
Bestimmung der Verzögerungszeit T0 des Robotersystems
Um eine möglichst lange Flugzeit des Tischtennisballs zu erreichen und so ausreichend Zeit
für die Prädiktion der Flugkurve zu erhalten, muss der Tischtennisball mit einem möglichst
großen Impuls senkrecht nach oben gespielt werden. Da für den Impuls gilt p = m · v, muss
der Ball bei jedem Auftreffen auf den Schläger mit der maximal möglichen Geschwindigkeit
die der Roboter fahren kann angespielt werden. Es stellt sich somit die Frage, wann der
Roboter ein Stellsignal erhalten muss damit die maximale Geschwindigkeit erreicht werden
kann. Um diese Verzögerungszeit zu ermitteln bietet sich die Analyse der Sprunganwort des
Systems an. Neben der Sprungantwort soll in diesem Aufgabenteil zudem die Ansteuerung
des Roboters und das Systemverhalten genauer analysiert werden.
4.2.1
Aufgabe
Das Hochfahren des Roboters und des Rechners erfolgt durch den Betreuer. Für die Ansteuerung des Roboters kommt das Python Programm Desktop\pprobot\move.py zum EinF–8
Z-Achse
X-Achse
Y-Achse
Draufsicht
Abbildung 3: Koordinatensystem
satz. Zum Bearbeiten des Programms kann der Editor IDLE verwendet werden. In diesem
Programm kann ein zeitlicher Ablauf an Punkten, welche der Roboter abfahren soll, eingegeben werden (Punkt-zu-Punkt Steuerung).
X, Y und Z stehen hierbei für die Koordinaten eines kartesischen Koordinatensystems (Abbildung 3), A, B und C für die Winkel die für den Tischtennisschläger eingestellt werden
können. Der Ursprung des Koordinatensystems befindet sich im Fußpunkt des Roboters. Die
Z-Koordinate steht hierbei für die vertikale Koordinatenachse, X- und Y-Koordinate für die
horizontale Ebene. Die Einheit der anzufahrenden Punkte ist mm, die Winkel müssen in
Grad angegeben werden. Die Zeit t wird in Sekunden angegeben. Die Eingabe von Fließkommazahlen (float) ist für alle Werte ebenfalls möglich. Die Ansteuerung kann über die
Angabe kompletter Koordinaten vorgenommen werden, oder aber nur der sich ändernden
kartesischen Koordinatenachsen. Eine mögliche Ansteuerung ist im Folgenden kurz dargestellt.
Trotz der Beschränkung der Stellsignale ist eine Kollision des Roboters mit dem Hallenboden und mit sich selbst nicht ausgeschlossen! Besonders das Verfahren in mehrere
Richtungen gleichzeitig kann zu Kollisionen führen! Auch wenn das Programm kollisionsfrei abläuft, ist eine Kollision des Roboters beim Zurückfahren in seine Startposition
möglich! Die Stellgrößen müssen daher mit großer Vorsicht gewählt werden!
Beispiel 1: Angabe der vollständigen Koordinaten
r.point(t
r.point(t
r.point(t
r.point(t
=
=
=
=
1,
2,
3,
4,
X
X
X
X
=
=
=
=
650,
950,
950,
650,
Y
Y
Y
Y
=
0, Z =
= 200, Z =
= 200, Z =
=
0, Z =
Beispiel 2: Angabe der sich ändernden Achsen
F–9
200,
300,
300,
200,
A
A
A
A
=
=
=
=
0,
0,
0,
0,
B
B
B
B
=
=
=
=
0,
0,
8,
0,
C
C
C
C
=
=
=
=
0)
8)
0)
0)
X-Achse Y-Achse Z-Achse
min 600 mm -500 mm 150 mm
max 1000 mm 500 mm 800 mm
Winkel A Winkel B
-20◦
-10◦
20◦
20◦
Winkel C
-20◦
20◦
Tabelle 1: Arbeitsraum
r.point(t
r.point(t
r.point(t
r.point(t
=
=
=
=
1,
2,
3,
4,
X
X
C
X
= 650, Y =
0, Z = 200)
= 950, Y = 200, Z = 300, C = 8)
= 20, B =
8, C =
0)
= 650, Y =
0, Z = 200, B = 0)
Beispiel 3: Angabe der Koordinaten im ’float’ Format
r.point(t = 1.2, X = 650. , Y =
r.point(t = 2.0, X = 900.5, Y =
r.point(t = 2.5, X = 650. , Y =
0, Z = 200.)
0, Z = 300.5)
0, Z = 200.)
Schreiben Sie nun verschiedene Programmabläufe und analysieren Sie das dynamische Verhalten des Roboters. Um Kollisionen mit dem Boden oder dem Roboter zu vermeiden, wurde
für die Systemanalyse ein quarderförmiger Arbeitsraum, der in Tabelle 1 definiert ist, angelegt. Trotz der Einschränkungen ist eine Kollision des Roboters mit sich selbst, aber auch mit
dem Hallenboden nicht ausgeschlossen, da der Bahnweg des Roboters zwischen verschiedenen Punkten nicht bekannt ist.
Nach Ablauf des Programmes öffnen sich selbsttätig drei Fenster die den Verfahrweg über
die Zeit in allen drei Koordinatenachsen visualisieren.
Abbildung 4 zeigt exemplarisch den zurückgelegten Weg in der Z-Komponente eines zuvor
ausgeführten Programms. Die Größe und der Zeitpunkt der Stellsignale werden durch rote
Punkte gekennzeichnet. Die Angabe der Position erfolgt in mm die der Zeit in s. Verwenden
Sie für das Ablesen der Verzögerungszeit ggf. auch den Zoom um ein möglichst genaues
Ergebnis zu erzielen.
4.2.2
Fragen
Beantworten Sie mit Hilfe ihrer Erfahrung, die Sie durch programmieren und analysieren
verschiedener Programmabläufe gesammelt haben, folgende Fragen:
1. Wo befinden sich die Drehachsen für die Winkel A, B und C?
2. Beeinflussen alle Winkel die Flugkurve des Balls?
3. Was ist der Zusammenhang zwischen Bewegungskurve und Geschwindigkeit des Roboters?
4. Warum ist die Ermittlung der Verzögerungszeit T0 notwendig?
5. Wie groß ist die Verzögerungszeit T0 ?
F – 10
Abbildung 4: Z-Koordinate
6. Hängt die Verzögerungszeit von der Roboterposition (Randbereiche) ab?
7. Wie ist das Systemverhalten bei Stellsignalen in sehr kurzen Abständen?
Notizen:
F – 11
Bildverarbeitung
b
Physik des Balles
b̂
Tracker /
Prädiktor
t̂i
Impulsregler
∆z
v̂i
r̂i
Regler für
vertikale
Reflexion
ϕ̂v,i
+
rs
Sample /
Hold
rs , ϕs
Roboter
r, ϕ
Abbildung 5: Blockschaltbild mit 2 Reglern. Verzögerunszeit T0 steckt im Impulsregler
5
Regelung des Industrieroboters
Um die Stabilität des Systems aus Tischtennisball und Roboter zu gewährleisten, ist eine
fortlaufende Regelung nötig. Die auf den Tischtennisball einwirkende Stellgröße ist der
Tischtennisschläger bzw. die Bewegung des Roboters. Da die Geschwindigkeit des Tischtennisschlägers durch das komplexe Systemverhalten des Roboters und dem Wunsch nach
einer möglichst langen Flugzeit des Balls als Regelgröße ausscheidet, erfolgt die Regelung
ausschließlich über die Winkel des Tischtennisschlägers. Diese müssen für jeden Aufprall
des Tischtennisballs auf den Tischtennisschläger neu berechnet werden. Eine Regelung des
Tischtennisballs ist nur in der horizontalen Ebene vorgesehen.
Abbildung 5 zeigt den schematischen Aufbau der Regelung des Ping-Pong-Roboters wie
er am Ende dieses Kapitels aussehen soll. Viele der Komponenten sind gleich wie in der
vereinfachten Abbildung 2. Während der Flugphase des Tischtennisballs ermittelt ein Stereokamerasystem mit einer Bildrate von 60 Hz die Ballposition b und speichert diese. Im
Anschluss wird die Ballposition aus der detektierten Position des Balls in den jeweiligen
Kamerabildern über Triangulation ermittelt. Da es sich bei der ermittelten Position um einen
Schätzwert handelt wird dieser als b
b bezeichnet. Bei der Berechnung der Ballposition stehen nur Bilder an diskreten Zeitpunkten zur Verfügung, was zu einer Diskretisierung der
Ballposition führt.
Da der Roboter schon in der Flugphase des Tischtennisballs ausgerichtet werden muss, ist
eine Vorhersage (Prädiktion) der Auftreffposition, Auftreffgeschwindigkeit und des Auftreffzeitpunkts nötig. Diese Prädiktion erfolgt durch ein Kalmanfilter. Das Kalmanfilter schätzt
mit Hilfe der 3D-Messpunkte aus der Bildverarbeitung die Parameter eines Systemmodells,
welches die Flugbahn des Tischtennisballs beschreibt. Setzt man diese Parameter in das Systemmodell ein, können die Auftreffposition rbi , Geschwindigkeit vbi und Zeitpunkt tbi vorhergesagt werden. Der Index i steht für das englische Wort impact (auftreffen).
Die regelnde Komponente ist in diesem Schaubild im Detail dargestellt und setzt sich aus
zwei Reglern zusammen. Als erstes betrachten wir den Impulsregler. Er erhält als EingangsF – 12
Stellsignal-Up
∆T
Stellsignal-Down
Abbildung 6: Zeit ∆T zwischen Stellsignalen
größe t̂i und hat zwei freie Parameter: T0 und δz. Die Ausgangsgröße des Impulsreglers
berechnet sich nach Formel (1).
" #
∆z = σ t − (t̂i − T0 ) − σ t − (t̂i − T0 + ∆T ) δz
(1)
Dieser Regler verändert die Sollposition des Roboters entlang der Z-Achse. Zur Zeit t − T0
vor dem geschätzten Auftreffen soll der Roboter mit einer Impulsbewegung entlang der ZAchse beginnen, damit er beim Auftreffen seine maximale Geschwindigkeit erreicht hat.
Das optimale T0 wurde im letzten Abschnitt experimentell ermittelt. Danach soll er sofort
wieder nach unten fahren um den nächsten Schlag abzuwarten. Dazu wird das Z-Stellsignal
∆T Sekunden später wieder um δz verringert. Das Stellsignal für die Abwärtsbewegung
wird bereits während der Aufwärtsbewegung an den Roboter übermittelt. Der Roboter fährt
hierbei in Z-Richtung eine parabelförmige Bahn deren Zenit δz über ihrem Startwert liegt
(Abbildung 6).
Der zweite Regler ist für die vertikale Reflexion verantwortlich und soll im Folgenden implementiert werden. Sein Eingang ist die geschätzte Auftreffgeschwindigkeit vbi , aus deren
Komponenten der Regler die für den Tischtennisschläger einzustellenden Winkel berechnet,
um den Ball vertikal nach oben zu spielen. Ausgangsgröße des Reglers sind daher die Winkel des Tischtennisschläger A, B, und C, welche zu einem Vektor ϕ̂v,i (Indizes stehen für
Vertikale Reflexion bei Impact) zusammengefasst wurden.
5.1
Jonglieren eines Tischtennisballs
Bevor eine Regelung des Tischtennisballs in eine optimale Position stattfinden kann, muss
der Tischtennisball durch den Roboter angenommen und eine gewisse Zeit in der Luft gehalten werden. Der Ball muss folglich zunächst senkrecht nach oben gespielt werden. Da
F – 13
Z - Achse
Ball
X - Achse
Abbildung 7: Tischtennisball
der Tischtennisball einen sich mit der Zeit t verändernden Geschwindigkeitsvektor v(t) besitzt, muss dieser zunächst durch das Kalmanfilter ermittelt werden. Mit dieser Information
kann der Prädiktor eine Schätzung der Auftreffgeschwindigkeit vornehmen. Dieser dreidimensionale Geschwindigkeitsvektor stellt die Basis für die Berechnung der einzustellenden
Tischtennisschlägerwinkel dar. Es müssen hierfür nur die Winkel bestimmt werden, welche
ein Verdrehen um die X- oder Y-Achse bewirken (horizontale Achsen). Der dritte Winkel,
welcher das Verdrehen des Tischtennisschlägers um die Z-Achse (vertikale Achse) bewirkt,
kann als konstant angenommen werden, da dieser keine Einwirkungen auf den reflektierten
Geschwindigkeitsvektor des Balls hat.
5.1.1
Aufgabe (schriftlich vorzubereiten)
Winkelberechnung: Abbildung 7 zeigt den Tischtennisball kurz vor dem Auftreffen auf den
Tischtennisschläger. Aus Gründen der Übersicht wurde auf eine dreidimensionale Darstellung verzichtet. Der Ball zeigt beim Auftreffen auf den Schläger näherungsweise das gleiche
Verhalten wie es ein Köper beim elastischen Stoss zeigt. D.h. auch hier kann das Reflexionsgesetz (Einfallswinkel = Ausfallswinkel) angewendet werden.
vsoll = v − 2(v T n) n
(2)
Der Einfallswinkel entspricht dem Winkel zwischen dem Normalenvektor n des Tischtennisschlägers und dem Geschwindigkeitsvektor v des Balls. Der Ausfallswinkel entspricht
dem Winkel zwischen Normalenvektor n des Tischtennisschlägers und dem nach oben gerichteten Einheitsvektor vsoll .
F – 14
Zeigen Sie mit Hilfe des Reflexionsgesetz, dass gilt:
vsoll − v
n= p
2(||v||2 − v T vsoll )
B = arctan
nx
,
nz
C = arctan
(3)
ny
,
nz
wobei n = (nx , ny , nz )T
(4)
Hinweis: Erweitern Sie Gleichung (2) mit v T und ersetzen Sie dann v T n in der ursprünglichen Gleichung (2).
Notizen:
Implementierung der vertikalen Reflexion: Öffnen Sie das Programm Desktop
\pprobot\pprobot.py und implementieren Sie den von ihnen gefundenen physikalischen Sachverhalt an den markierten Stellen. Da der Roboter eine Winkelangabe in Grad
benötigt ist ggf. eine Umrechnung nötig. Starten Sie im Anschluss mit Hilfe des Betreuers
das Programm Desktop\pprobot\pprobot.py sowie den Roboter und prüfen Sie, ob
der Roboter die von ihnen zugeworfenen Tischtennisbälle korrekt nach oben spielt. Es ist
für diese Aufgabe sinnvoll, T0 = 0 zu setzen, da so das Verhalten des Balles mit weniger
Störeinfluss zu sehen ist.
Die von ihnen für das Programm benötigten Python Befehle bzw. Funktionen sind:
• atan2(a,b) - Berechnet den Arcustangens
• pi - π
• (x)**.5 - Berechnet die Quadratwurzel von x
F – 15
Informationen zur Python Hilfe oder dem Arbeiten mit Vektoren und Matrizen können dem
Anhang entnommen werden.
Implementierung der Verzögerungszeit: Sobald die Berechnung der Winkel für den Tischtennisschläger des Roboters korrekt erfolgt, kann der Roboter bewegt werden. Hierfür ist
die Implementierung der von ihnen ermittelten Verzögerungszeit T0 erforderlich. Implementieren Sie im Programm Desktop\pprobot\pprobot.py die von ihnen ermittelte
Verzögerungszeit an der markierten Stelle. Starten Sie den Roboter erneut und testen Sie ihr
Ergebnis. Der Roboter sollte den Ball einige Male jonglieren, bevor dieser den Arbeitsraum
verlässt.
5.2
Parameter eines PID Reglers
In der zuvor bearbeiteten Aufgabe wurde der Tischtennisball bisher senkrecht nach oben
jongliert. Unbekannte Störgrößen – wie zum Beispiel der Drall des Balles – und Schätzfehler bringen eine Abweichung in die Flugbahn des Balles. Da der Roboter aber nur einen sehr
beschränkten Armradius hat verliert er den Ball schnell: das System ist also instabil. Um den
Ball in einem optimalen Abstand für den Roboter zu halten ist eine zusätzliche Regelung
notwendig. Aufgabe der neuen Regelung ist es, den Tischtennisball in eine Position zu regeln, in der der Roboter über einen großen Dynamikbereich in seinen Bewegungen verfügt
und so auch große Störungen ausregeln kann.
Neben der Stabilität werden zudem Anforderungen an Schnelligkeit, Dämpfung und stationäre Genauigkeit gestellt. Diese Randbedingungen sollten möglichst optimiert werden,
bzw. müssen eine bestimmte Güte erfüllen.
Für den Reglerentwurf und die Ermittlung der Regelparameter ist für gewöhnlich eine Modellierung der Strecke (Prozess) notwendig. Das Zusammenwirken aus Ball und Roboter
führt aber auf ein sehr komplexes Systemverhalten, welches eine Identifikation des Prozesses erschwert. Es steht daher für die Regelung kein Modell der Strecke zur Verfügung. Trotz
der fehlenden Übertragungsfunktion soll aber ein Regler ausgewählt werden, der den Anforderungen nach Stabilität, Schnelligkeit und stationärer Genauigkeit genügt. Prinzipiell
könnten verschiedenste Regler zum Einsatz kommen. Um dem Verlangen nach stationärer
Genauigkeit entgegen zu kommen sollten diese (auch wenn nicht zwingend notwendig) über
ein I-Glied (Integrierglied) verfügen. Ein reiner I-Regler eignet sich aber nur bedingt, da
die Stabilität des Regelkreises nur unter bestimmten Randbedingungen gegeben ist und er
zudem sehr langsam ist.
Um die Vorteile des I-Glieds zu nutzen, kann dieses mit den Eigenschaften anderer Regler kombiniert werden. Eine Möglichkeit besteht darin das I-Glied mit einem P-Glied zu
kombinieren. Das P-Glied (Proportionalglied) dient hierbei als Verstärkung und beschleunigt den Regelkreis. Weitere Glieder die in der Regelungstechnik zum Einsatz kommen sind
D-Glieder (Differenzierglied). Ein D-Glied reagiert schnell auf Änderungen am Eingang und
ist als alleiniger Regler nicht geeignet.
Da die zu regelnde Strecke nicht bekannt ist müssen die Parameter des Reglers empirisch
ermittelt werden. Um ein möglichst breites Spektrum an Möglichkeiten der Regelung zu
erreichen, bietet sich der Einsatz eines PID-Reglers an. Dieser besteht aus der Kombination
F – 16
Bildverarbeitung
b
Physik des Balles
b̂
t̂i
Tracker /
Prädiktor
Impulsregler
∆z
v̂i
r̂i
Regler für
vertikale
Reflexion
ri,soll
+
ϕ̂v,i
+
+
rs
ϕs
Sample /
Hold
rs , ϕs
Roboter
r, ϕ
∆ϕ
PID
r
Abbildung 8: Regelkreise mit allen drei Reglern
von jeweils einem P-,I- und D-Glied. Auch wenn dieser ggf. nicht die optimale Regelung liefert, kann die Anpassung des Systems über verschiedene Parameter erfolgen und so schneller
ein akzeptables Ergebnis liefern.
Abbildung 8 zeigt die Implementierung des zusätzlichen Reglers. Er nimmt als Eingang den
Unterschied zwischen der Ist-Auftreffposition und der Soll-Auftreffposition in mm und gibt
als Ausgang eine Veränderung der Stellwinkel weiter. Diese werden direkt auf das Ergebnis
des Reglers für vertikale Reflexion aufaddiert und so an das Sample / Hold Glied weiter
gegeben.
Da es sich um ein zeitdiskretes System handelt, muss dieses durch einen zeitdiskreten PIDRegler geregelt werden. Die Stellgröße ∆ϕ des PID-Reglers berechnet sich für die X-Achse
zu:
∆ϕ = PG · ∆x + IG
N
X
0
∆x · ∆t + DG
∆xcurrent − ∆xlast
∆t
(5)
∆x ist hierbei der Eingang, ∆t bezeichnet die Diskretisierungseinheit der Zeit.
5.2.1
Aufgabe
Da die Parameter des PID-Reglers nicht auf analytischem Wege bestimmt werden können,
sollen diese durch experimentelles Vorgehen ermittelt werden. Öffnen Sie hierfür das Programm Desktop\pprobot\pprobot.py mit einem Editor und geben Sie plausible
Werte ein. Starten Sie (nur unter Aufsicht des Betreuers) den Roboter und beobachten Sie
das Verhalten des Roboters. Analysieren Sie den Einfluss der verschiedenen Parameter und
versuchen Sie durch Verändern der Parameter eine möglichst optimale Regelung zu gewährleisten.
F – 17
5.2.2
Fragen
Beantworten Sie folgende Fragen mit Hilfe der Erfahrungen, welche Sie durch das Experimentieren mit dem PID-Regler gesammelt haben. Achten Sie insbesondere auf den Einschwingvorgang (z.B. Überschwinger).
1. Welche Störgrößen wirken auf das System ein?
2. Welche Größenordnung erwarten Sie für die Paramter ’P’, ’I’ und ’D’?
3. Welchen Einfluss haben die Parameter ’P’, ’I’ und ’D’ auf das Verhalten des Systems?
4. Warum ist ein PID-Regler für diese Regelungsaufgabe geeignet?
5. Welche Werte haben Sie für die Parameter P, I und D ermittelt?
Notizen:
F – 18
A
A.1
Programmieren mit Python
Datentypen und Operatoren
Python verfügt über eine Vielzahl verschiedener Datentypen. Die in diesem Versuch am
häufigsten verwendeten Datentypen können Tabelle 2 entnommen werden. Bei der Deklaration einer Variablen ist es, im Gegensatz zu Programmiersprachen wie z.B. C++, nicht
notwendig den Datentyp der Variable zu definieren. Python kann dynamisch den Datentyp
einer Variable erkennen und diesen im späteren Verlauf sogar ändern.
Die folgenden Beispiele zeigen, worauf beim Arbeiten mit verschiedenen Datentypen zu
achten ist:
Beispiel 1: Integer
>>> 7/2
3
#Division mit Datentyp integer
Da es sich bei den Zahlen 7 und 2 um Zahlen des Typs Integer handelt, folgt auch eine
Ausgabe vom Typ Integer; d.h. die Zahl wird immer abgerundet.
Beispiel 2: Float und Integer
>>> 7./2
3.5
>>> 7/2.
3.5
#Division mit Datentyp float und integer
#Division mit Datentyp integer und float
Auch wenn nur eine Zahl vom Typ float ist, ist das Ergebnis mathematisch korrekt.
A.2
Bibliotheken und Hilfe
Für das Arbeiten mit Grafiken, mathematischen Funktionen oder Matrizen kann auf ein breites Spektrum an Bibliotheken zurückgegriffen werden. In diesen Bibliotheken sind bereits
diverse Funktionen definiert und können in Programme eingebunden werden.
Integer
Float
String
List
a=1
Ganze Zahlen
b = 0.1
Gleitkommazahlen
c = ’Hallo’
Zeichenkette
l = [1, 2, 3] Veränderbare Liste
Tabelle 2: Datentypen
F – 19
A.2.1
Bibliotheken
Da es für Python eine Vielzahl verschiedener Bibliotheken gibt, werden im Folgenden die
Wichtigsten kurz vorgestellt.
numpy: Stellt Funktionen und Operatoren für das Arbeiten mit Matrizen zur Verfügung.
Häufig importierte Funktionen sind: array, cross, dot, empty, zeros, identity und diag.
math: Bietet ein breites Spektrum an mathematische Funktionen und Operatoren an. Häufig
importierte Funktionen sind: pi, atan, atan2, cos, sin, sqrt.
matplotlib: Ermöglicht das Erstellen von dynamischen Plots.
Beispiel: Importieren aus Bibliotheken
>>> from numpy import array, dot, cross
>>> a = array([1,2,3]) #Vektor a
>>> b = array([3,2,1]) #Vektor b
>>> cross(a,b)
#Vektorprodukt
array([-4, 8, -4])
>>> dot(a,b)
#Skalarprodukt
10
Möchte man alle Funktionen von z.B. NumPy importieren kann man auch from numpy
import * eingeben. Dies funktioniert analog für andere Bibliotheken.
A.2.2
Python Hilfe
Um die Funktionsweise eines Befehls zu erfahren, kann zudem auf die Python Hilfe zurückgegriffen werden. Geben Sie hierfür help(Befehl) in die Python Konsole (öffnet sich
beim Starten des Editors IDLE) ein. Sollte eine Fehlermeldung erscheinen z.B. Object
‘atan2‘ not found muss die Funktion ggf. noch importiert werden.
Beispiel: Hilfe
>>> from math import atan2
>>> help(atan2)
Help on built-in function atan2 in module math:
atan2(...)
atan2(y, x)
Return the arc tangent (measured in radians) of y/x.
Unlike atan(y/x), the signs of both x and y are considered.
A.3
Ein- und Ausgabe
Für die Ausgabe gibt es in Python den Befehl print“.
”
F – 20
>>> a=2
>>> print a
2
>>> a = "Hallo"
>>> print a
Hallo
Bei der Eingabe muss der Datentyp spezifiziert werden.
>>>a = raw_input()
#Eingabe für Strings
Hallo
>>>b = int(raw_input()) #Eingabe für integer
2
>>>print a
Hallo
>>>print b
2
A.4
Beispiele für Verzweigungen und Schleifen
Beispiel: if Anweisung
>>>
>>>
...
...
...
...
...
...
...
x = int(raw_input("Gib eine Zahl ein: "))
if x < 0:
x=0
print ("Negative Zahl zu Null geändert.")
elif x == 0:
print "Null!"
else:
print "Positive Zahl!"
Beispiel: for Anweisung
>>> i =
>>> for
...
...
0
i in range(10):
i +=1
print i
Mit der Funktion range kann eine Sequenz von ganzen Zahlen erzeugt werden.
Beispiel: while Anweisung
>>> a, b = 0, 1
>>> print a
>>> while b < 10:
...
print b
...
a, b = b, a+b #Fibonacci Folge
F – 21
A.5
Funktionen
Funktionen werden in Python durch das Schlüsselwort def“ definiert. Wichtig: Alle Linien
”
einer Funktion müssen dieselbe Einrückung haben!
Beispiel: Funktionen
>>> def Skalarprodukt(a,b):
...
res = a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
...
return res
>>>
>>>
>>>
>>>
14
>>>
14
from numpy import array, dot
a = array([1,2,3])
b = array([1,2,3])
Skalarprodukt(a,b)
dot(a,b)
Für die Definition kleiner Funktionen kann auch eine Closure (kurze, namenlose Funktion)
eingesetzt werden. Diese werden in Python mit dem Schlüsselwort lambda definiert. Wichtig: Die Lambda-Closures beinhaltet, im Gegensatz zu gewöhnlichen Funktionen keinen return Aufruf
Beispiel: Lambda Closure
>>>
>>>
5
>>>
>>>
>>>
>>>
>>>
14
>>>
14
A.6
addition = lambda a,b: a+b
addition(2,3)
Skalarprodukt = lambda a, b: a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
from numpy import array, dot
a = array([1,2,3])
b = array([1,2,3])
Skalarprodukt(a,b)
dot(a,b)
Vektoren und Matrizen
Möchte man Vektoren oder Matrizen darstellen, so müssen ggf. Bestandteile der numpy
Bibliothek importiert werden.
Beispiel: Erstellen von Vektoren und Matrizen
>>>from numpy import array
>>>a = array([1,2,3]) #Vektor mit 3 Elemten
F – 22
>>>a[0]
#Zugriff auf das 1. Element
1
>>> b = array([[1,2,3],[1,2,3],[1,2,3]]) #Matrix
>>> b
array([[1, 2, 3],
[1, 2, 3],
[1, 2, 3]])
Beispiel: Transponierte Vektoren und Matrizen
>>>from numpy import array
>>>b = array([[1,2,3],[1,2,3],[1,2,3]]) #Matrix
>>>b
array([[1, 2, 3],
[1, 2, 3],
[1, 2, 3]])
>>>b.T
array([[1, 1, 1],
[2, 2, 2],
[3, 3, 3]])
Beispiel: Matrix Multiplikation
>>>from numpy import eye, dot, array
>>>b = array([[1,2,3],[1,2,3],[1,2,3]])
>>>c = eye(3) # erzeugt eine 3x3 Einheitsmatrix
>>>dot(b,c)
array([[ 1., 2., 3.],
[ 1., 2., 3.],
[ 1., 2., 3.]])
>>>b*c
array([[ 1., 0., 0.],
[ 0., 2., 0.],
[ 0., 0., 3.]])
b*c ist die Hadamard (Elementweise) Multiplikation.
F – 23