Dutch Am Robot

Studienarbeit Ansteuerung
eines Roboterarms
FH Mannheim Christian von der Heyd
Fachbereich U Thomas Ster
Studienarbeit
Untersuchungen zur geregelten Positionierung
und Bahnplanung eines 5-achsigen
Roboterarms ber eine SPS
Student: Christian von der Heyd
Mittelgewann 20
68723 Schwetzingen
vonderHeyd@web.de
Matrikelnummer: 9921147
Student: Thomas Ster
Auf der Weide 7
67259 Groniedesheim
thomas.stoesser@web.de
Matrikelnummer: 9920216
Studiengang: Fachbereich Automatisierungstechnik
Fachhochschule Mannheim
Hochschule fr Technik und Gestaltung
Zeitraum: 17. Februar bis 17. Mai 2003
Betreuer: Prof. Dr. Ing. M. Seitz
Dipl.-Ing. H. Peter
Institut fr Elektronische Steuerungstechnik
Studienarbeit Ansteuerung Erklrung
eines Roboterarms
FH Mannheim Christian von der Heyd Erklrung
Erklrung
Hiermit erklren wir, da wir die vorliegende Arbeit selbstndig angefertigt und alle
benutzten Quellen und Hilfsmittel vollstndig und genau angegeben haben. Wir
haben alles kenntlich gemacht, was aus Arbeiten Anderer unverndert oder mit
nderungen bernommen wurde.
------------------------------ --------------------------
Christian von der Heyd Thomas Stsser
Ort, Datum
Studienarbeit Ansteuerung Vorwort
eines Roboterarms
FH Mannheim Christian von der Heyd Vorwort
Vorwort
Zum Inhalt des Studiums der Automatisierungstechnik gehrt die Durchfhrung einer
Studienarbeit, die unter anderem als Vorbereitung auf die sptere Diplomarbeit dient.
In dieser Studienarbeit kann der Student demonstrieren, da er die erlernten
Werkzeuge eines Ingenieurs anwenden kann. Im Verlaufe seines Studiums obliegt
es dem Studenten nun, sich um ein geeignetes Thema zu bemhen. In diesem Sinne
entschieden wir uns fr die vorliegende Aufgabenstellung, die uns Herr Prof. Dr. Ing.
Matthias Seitz anbot. Die Bearbeitung des Themas erfolgte im Institut fr
Steuerungstechnik.
An dieser Stelle mchten wir nun unseren besonderen Dank an Herrn Prof. Dr. Ing.
Matthias Seitz richten, der uns die Ausfhrung dieser Studienarbeit ermglichte und
uns in jeder Situation mit Rat und Tat zur Seite stand.
Gleichermaen mchten wir uns bei Herrn Dipl.-Ing. Hans Peter fr seine stetige
Hilfsbereitschaft bedanken. Mit seiner Untersttzung konnten wir jederzeit rechnen.
Er half uns schnell und kompetent angefallene Probleme aus der Welt zu schaffen.
Die Autoren
Christian von der Heyd Thomas Ster
Studienarbeit Ansteuerung Zussammenfassung
eines Roboterarms
FH Mannheim Christian von der Heyd Zusammenfassung
Zusammenfassung
Die Entwicklung von Robotern war in der Vergangenheit und wird auch in naher
Zukunft eine wesentliche Rolle in der Automatisierungstechnik spielen. So werden
heute Produktionsablufe zunehmend durch Roboter realisiert. Die sich dadurch
ergebenden Vorteile wie Przision sowie Kosten- und Zeitersparnis erklren sich von
selbst. Ein klassisches Beispiel stellen hier schon die Fertigungsstrassen der
Automobilindustrie dar.
Den Hauptvertreter aus der Gruppe der industriellen Fertigungsroboter stellen
Roboterarme dar, die menschliche Bewegungsablufe nachahmen sollen. Dabei
ermglichen geregelte Antriebe die Positionierung der Roboterachsen. Das
Hauptproblem in der Robotik besteht allerdings nicht in der dynamischen Regelung
der Gelenkwinkel, sondern in der Bewegungsteuerung des Roboters im kartesischen
Raum.
Die Aufgabe dieser Studienarbeit ist es in diesem Zusammenhang, die
Positionierung des Greifers im Raum durch eine geregelte Bahnplanung zu
realisieren. Die Grundlagen hierfr sind Thema der Vorlesung SP2.
Diese Arbeit baut auf der Diplomarbeit von Herrn Christoph Hyllus auf. Seine
Aufgabe war die Bedienung des Roboters ber einen Leitrechner. Das
Steuerprogramm wurde mit der Entwicklungsumgebung Prosys erstellt und lief auf
einer Soft-SPS ab. Die Kommunikation zum Roboter erfolgte ber ein selbst
entwickeltes C-Skript. Die Verbindung zwischen SPS und C-Anwendung wurde ber
eine DDE-Schnittstelle realisiert.
In dieser Arbeit soll nun eine direkte Verbindung zwischen SPS und Roboter via
RS232-Schnittstelle hergestellt werden. Dazu stellt die neue Entwicklungsumgebung
CoDeSys spezielle Kommunikationsbausteine zur Verfgung, die in das
Anwenderprogramm eingebunden werden knnen. Ein entsprechendes
Ansteuerprogramm ist zu erstellen. Als problematisch gestaltet sich dabei die
Ermittlung der Winkelstellungen aus den Positionssollwerten: Auf Grund von
Singularitten kann es bei der Berechnung zu Mehrdeutigkeiten kommen, da der
Roboter mehrere Winkel einnehmen kann, um ein und den selben Punkt zu
erreichen. Zur Steuerung des Roboterarms im kartesischen Raum soll aus diesem
Grund eine differentielle Winkelbetrachtung mit Linearisierung im Arbeitspunkt
durchgefhrt werden. Hierfr sind verschiedene mathematische Anstze umzusetzen
( Denavit- Hartenberg-Modell, Vorwrtstransformation, Rckwrtstransformation,
Jakobi-Matrix, Invertierung einer Matrix ).
Zur Vermeidung von Positionierfehlern auf Grund der Linearisierung ist eine
Bahnplanung zu realisieren, die den Greifer des Roboters schrittweise an die
Sollposition heranfhrt.
Alternativ zur Bahnplanung wird ein Lsungsansatz untersucht, der basierend auf
trigonometrischen berlegungen absolute Winkelsollwerte liefern soll.
Die Schnittstelle zum Bediener bildet eine Visualisierung, ber die Sollwerte
vorgegeben werden knnen und die ber den Betriebszustand des Roboters
informiert.
Studienarbeit Ansteuerung Inhalt
eines Roboterarms
FH Mannheim Christian von der Heyd Inhalt
Inhaltsverzeichnis
Inhalt: Seite:
1 Einleitung
1.1 Geschichte von Robotern 1.1
1.1.1 Entwicklung von Robotern 1.2
1.1.2 Anwendungsgebiete von Robotern 1.3
1.1.3 Zukunftsaussichten fr Roboter 1.5
1.2 Aufgabestellung 1.6
1.2.1 Hauptaufgaben der Studienarbeit 1.6
1.2.2 berblick 1.7
2 Grundlagen
2.1 Aufbau eines Industrieroboters 2.1
2.2 Kinematische Beschreibung des Manipulators 2.2
2.3 Aufbau des Roboterarms ROB 3 2.3
2.4 Arbeitsraum des Roboterarms ROB 3 2.4
3 Serielle Kommunikation zwischen
Steuerung und Roboterarm
3.1 Beschreibung der Schnittstellenbausteine 3.2
von CoDeSys
3.2 Beschreibung des Datenaustauschprotokolls 3.4
3.2.1 Kommunikationsaufbau 3.4
3.2.2 Senden der Steuerdaten 3.4
3.2.3 Ansteuerkommandos fr den Roboterarm 3.5
3.3 Testen der Verbindung 3.7
3.3.1 Test der Ansteuerung ber die serielle Schnittstelle 3.7
3.3.2 Aufgetretene Probleme beim Test der 3.8
seriellen Kommunikation
3.3.3 Probleme beim Test der Achsansteuerung 3.9
eines Roboterarms
Inhalt: Seite:
4 Kinematik der Position
4.1 Einleitung 4.1
4.2 Grundlagen der Kinematik 4.1
4.2.1 Einfhrung in die zugrundeliegende Matrizenrechnung 4.1
4.2.2 Regeln fr die Festlegung der Koordinatensysteme 4.3
4.2.3 Festlegung der Koordinatensysteme fr 4.5
den Roboterarm ROB3
4.2.4 Einfhrung in die notwendige Vektorrechnung 4.6
4.3 Bestimmung der absoluten Position und Orientierung 4.8
des TCP im Basiskoordinatesystem
4.3.1 Berechnung der A-Matrizen 4.8
4.3.2 Berechnung der Endmatrix
R
T
H
aus den A-Matrizen 4.13
4.4 Probleme und Test 4.16
4.5 Bestimmung der absoluten Position und Orientierung 4.17
aus der Matrix
R
T
H
4.6 Bestimmung der Jakobi-Matrix 4.19
4.7 Die Invertierte der Jakobi-Matrix 4.21
4.8 Probleme und Zusammenfassung 4.22
5 Alternativer Lsungsansatz
5.1 Einleitung 5.1
5.2 Lsungsansatz 5.1
5.3 Berechnung der Winkel 5.3
5.4 Realisierung der Winkelberechnung im Ansteuerprogramm 5.7
5.4.1 Anpassung des Greiferwinkels 5.7
5.4.2 berwachung der Berechnung 5.9
5.4.3 Anpassung der Greiferdrehung 5.9
5.5 Einschrnkungen 5.10
eines Roboterarms
Inhalt: Seite:
6 Programmbeschreibung
6.1 Beschreibung der ablaufrelevanten Programmteile 6.1
6.1.1 Baustein ROBOTERANST 6.1
6.1.2 Baustein ABSOLUTE_WINKELBERECHNUNG 6.1
6.1.3 Programm STOERUNG 6.2
6.1.4 Programm AUSWAHL 6.2
6.1.5 Programm INIT 6.3
6.1.6 Baustein INFORMATION 6.4
6.1.7 Baustein REFERENZ 6.4
6.1.8 HAUPTPORGRAMM 6.4
6.1.9 Programm AUFNAHME 6.5
6.1.10Programm ABLAGE 6.7
7 Visualisierung
7.1 Erstellen einer Visualisierung 7.1
7.2 Beschreibung der erstellten Visualisierung 7.1
7.3 Bedienung des Roboters 7.4
8 Schlusswort
8.1 Erkenntnisse 8.1
8.2 Ergnzungen 8.2
8.3 Ausblick 8.2
9 Literatur
eines Roboterarms
Anhang
Anhang A
SPSProgrammierung mit CoDeSys
Anhang B
Partielle Ableitung der absoluten Orientierung
Anhang C
Progammbeschreibung der Rckwrtstransformation
Anhang D
Beschreibung der Software Maple
Anhang E
Listings
Studienarbeit Ansteuerung Einleitung
eines Roboterarms
FH Mannheim Christian von der Heyd Seite 1.1
1 Einleitung
1.1 Geschichte von Robotern
[ nach Quelle 16 ]
Die Menschen haben schon immer davon getrumt, automatische Helfer zu haben,
die ihnen die Arbeit abnehmen. Im Mittelalter bis hin zum Anfang des
20. Jahrhunderts gab es immer wieder Versuche, solche Maschinen zu bauen. Diese
Maschinen, die teilweise Wunderwerke der Mechanik waren, hatten aber nie eine
praktische Bedeutung.
Der Begriff 'Roboter' wurde im Jahre 1920 von dem tschechischen Schriftsteller K.
Capek in seinem Schauspiel "RUR" geprgt. Er bezeichnete hiermit
Maschinenmenschen, die anstelle des Menschen an den Werkbnken stehen.
Abgeleitet hat er den Begriff vom slawischen Wort 'rabota' = Arbeit. Als Geburtsjahr
fr den praktischen Einsatz von Robotern, d.h. die Erfindung des Industrieroboters,
gilt das Jahr 1956. George C. Devor beantragte in diesem Jahr ein US-Patent fr die
"Programmierte bergabe von Artikeln", welches ihm im Jahre 1961 erteilt wurde.
Der erste Prototyp wurde 1958 von der "Devol Consolidated Control Corporation"
gebaut. Die ersten serienmigen Industrieroboter stellte dann Anfang der 60-er
Jahre die Firma Unimation her. Die anfnglich nur zgerliche Reaktion der Industrie
in den USA und den anderen Industrielndern nderte sich erst, als Japan 1968
massiv in das Robotergeschft einstieg. Seit dem geht die Entwicklung und der
Einsatz von Industrierobotern steil bergauf. Roboterachsen knnen rotatorisch oder
linear ausgelegt sein. In Abhngigkeit davon wie mehrere dieser Achsen miteinander
verbunden werden, ergeben sich unterschiedliche Arbeitsrume und Bezeichnungen
wie z.B. Knickarm [ Abbildung 1.1 ] oder Linear-Roboter [ Abbildung 1.2 ].
Beispiele:
Abbildung 1.1: Linear-Roboter Abbildung 1.2: Knickarm
eines Roboterarms
1.1.1 Entwicklung von Robotern
Ob frei von digitalen Codes, optimiert vom technischen Fortschritt oder angepat an
moderne Dienstleistung, die Entwicklung von Robotern geht sehr unterschiedliche
Wege. [ Quelle 11, Seite 3 ]
In der technischen Praxis kommen allerdings hauptschlich Industrieroboter zum
Einsatz, die nach VDI-Richtlinie 2860 ( von 1981 ) als universal einsetzbare
Bewegungsautomaten mit mehreren Achsen definiert werden. Die Bewegungen
dieser Roboter sind hinsichtlich Bewegungsfolge frei programmierbar.
Gegebenenfalls knnen sie ber Sensoren gefhrt werden. Zur Ausfhrung von
Handhabungs- und/oder Fertigungsaufgaben werden Industrieroboter mit Greifern,
Werkzeugen oder anderen Fertigungsmitteln ausgerstet.
Die industrielle Entwicklung und das Verlangen nach Erzeugnissen in groen
Stckzahlen fhrten zur Automatisierung von technischen Prozessen. Das
industrielle Zeitalter, insbesondere auch die Elektrotechnik, ist dabei geprgt vom
Verlangen nach Produktivittssteigerung und den damit verbundenen Fragen:
wie schnell und wie gnstig kann man ein Produkt herstellen, ohne dabei an Qualitt
zu verlieren.
Um dieses Ziel zu erreichen, mu eine Maschine mit mehr Kraft, unermdlich und
schnell menschliche Ttigkeiten nachahmen. In vielen Industriezweigen ist das fast
gelungen. Durch ausgefeilte Sensorik schaffte man Hallen voller Industrieroboter, die
je nach Bestellung und Produkt variabel programmiert werden knnen. Statt
dutzende Arbeiter, die oftmals einer monotonen Arbeit nachgingen, traten an diese
Stelle nun eine handvoll Techniker zur berwachung der Prozesse. Die wichtigsten
Grnde fr die Umstellung auf eine automatische Fertigung waren in diesem
Zusammenhang folgende Aspekte:
a) Wirtschaftlichkeit:
Die Schnelligkeit der Roboter mit ihrer Hebekraft kann von keinem Menschen ersetzt
werden. Dazu kommen die Lohnkosteneinsparungen, da der ein Roboter die Arbeit
von mehreren Menschen abnimmt.
b) Arbeitsschutz:
In vielen Industriebereichen kann es durch schdliche Stoffe wie
Sure oder Dmpfe sehr schnell zu Verletzungen der Arbeiter kommen.
c) Qualittssicherung:
Der Einsatz von Robotern gewhrleistet eine Konstanz bei hohen Stckzahlen (
Massenproduktion ) mit geringen Fertigungstoleranzen.
eines Roboterarms
d) Miniaturisierung:
Die Gre mancher Objekte ( siehe Chip Fertigung ) berschreitet die Grenzen
menschlicher Przision, was die Positioniergenauigkeit und die
Arbeitsgeschwindigkeit betrifft.
1.1.2 Anwendungsgebiete von Robotern
[ nach Quelle 16 ]
Zur Frage der Definition was ein Roboter ist, gibt es genauso viele Antworten wie
Roboter. Unterscheidungskriterien sind dabei beispielsweise Anzahl der Achsen,
Flexibilitt und Programmierbarkeit. Auerdem verlangt die jeweilige Anwendung
nach den unterschiedlichsten Ausfhrungen von Robotern.
Dieses Jahrhundert wird das Jahrhundert der Roboter werden. Roboter werden in
alle Bereiche des alltglichen Lebens eindringen, in die Industrie genauso wie in den
privaten Bereich. Der industrielle Einsatz von Robotern ist hierbei nur ein Teilgebiet.
Hier amortisieren sich Roboter meist nach 1 bis 2 Jahren.
Auch heute schon werden Roboter in der Industrie vielseitig eingesetzt: Sie erledigen
Arbeiten wie transportieren, stapeln, montieren, schrauben, beladen, entnehmen,
verpacken, sortieren, schneiden, schweien, gieen, strahlen, lten, dosieren,
reinigen, flmmen, kleben, dichten, sprhen, lackieren, schumen, palettieren,
bohren, schleifen, entgraten, messen, testen, prfen.
Sie erobern aber auch zunehmend alle anderen Bereiche des Alltags: Sie forschen,
spielen [ Abbildung 1.4 ] oder machen sauber. Sie knnen sehen, hren und reden.
Roboter knnen mittlerweile laufen, fliegen, klettern, schwimmen und tauchen. Auch
an Operationstischen [ Abbildung 1.3 ] kommen sie heute bei hochprzisen Eingriffen
zum Einsatz.
Im folgenden werden Anwendungsbeispiele fr den Einsatz von Robotern im Alltag
gezeigt.
eines Roboterarms
a) Roboter in der Medizintechnik:
[ nach Quelle 17 ]
Abbildung 1.3: Roboter beim
Bearbeiten einer Zahnprothese
b) Roboter in der Spielzeugindustrie:
[ nach Quelle 18 ]
Abbildung 1.4: Spielzeugroboter als Bausatz
eines Roboterarms
1.1.3 Zukunftsaussichten fr Roboter
[ nach Quelle 19 ]
Fr 2003 geht man davon aus, da es dann weltweit 892000 Industrieroboter geben
wird. Mit dieser Prognose ist damit schon vorsichtiger geworden, denn im Jahr zuvor
ging man noch davon aus, da es bis zum Jahr 2002 fast eine Million
Industrieroboter geben werde. Zum wachsenden Einsatz von Robotern trage der
Arbeitskrftemangel bei, aber auch die Notwendigkeit, da die Montage in der
Industrie mit einer hohen und konstanten Qualitt durchgefhrt werden msse, was
oft nur Roboter leisten knnten.
In Zukunft werden Roboter immer mehr das menschliche Leben beeinflussen.
Dienstleistungsroboter wie der dargestellte Staubsauger Roboter [ Abbildung 1.5 ]
werden in die Haushalte einziehen um die Arbeit im privaten Sektor zu erleichtern.
[ aus Quelle 20 ]
Abbildung 1.5: Staubsauger-Roboter Roomba
eines Roboterarms
1.2 Aufgabenstellung
1.2.1 Hauptaufgaben der Studienarbeit
Die Studienarbeit kann im Wesentlichen in folgende Abschnitte unterteilt werden:
a) Der Roboterarm soll ber die RS 232 Schnittstellen an ein Steuersystem
angekoppelt werden. Die Steuerung ist in Form einer Soft-SPS auf einem
Standard-PC implementiert. Dabei handelt es sich um ein Programm, das die
Funktionalitt einer realen SPS auf dem Betriebssystem des Rechners
nachbildet. Das notwendige Ansteuerprogramm soll in der
Entwicklungsumgebung CoDeSys realisiert werden.
b) Der Roboterarm soll automatisiert werden:
Die SPS berwacht und regelt den Bewegungsablauf des Roboters.
Hauptaufgabe ist dabei die Berechnung der notwendigen Winkelstellungen der
Roboterachsen aus den kartesischen Raumkoordinaten eines Punktes, der
angefahren werden soll. Hierfr sind mehrere mathematische
Berechnungsanstze notwendig ( Denavit-Hartenberg-Modell,
Vorwrtstransformation, Rckwrtstransformation, Bahnplanung,
Matrizenberechnung, Jakobi-Matrix ). Der Berechnungsalgorithmus arbeitet
bei der Ermittlung der Gelenkstellungen mit differentiellen Winkeln
c) Das Ansteuerprogramm soll mit Hilfe eines alternativen Lsungsansatzes
realisiert werden. Dieser beruht auf trigonometrischen Gleichungen, die unter
bestimmten Annahmen die absoluten Winkelstellungen ermitteln. Die
Ansteuerung kann gesteuert erfolgen.
d) Die Bedienung des Roboters soll auf dem Steuerrechner erfolgen. Hierfr ist eine
Visualisierung zu erstellen, ber die der Roboter angesteuert wird. Gleichzeitig
soll sie den Bediener ber den Betriebszustand informieren.
eines Roboterarms
1.2.2 berblick
Der folgende Abschnitt soll einen kurzen berblick ber die Inhalte dieses Berichtes
liefern. Er beschreibt die prinzipielle Vorgehensweise bei der Lsung der einzelnen
Aufgabenstellungen. Die Lsungsanstze sind dabei eng an die Abfolge der Kapitel
gebunden.
a) Kapitel 2 erlutert zunchst die allgemeine Theorie zur Beschreibung von
Industrierobotern. Ferner beschreibt Kapitel 2 den mechanischen Aufbau des
Roboterarms ROB 3 der Firma Eurobtec.
b) Im Kapitel 3 wird die Vorgehensweise beim Aufbau einer seriellen Kommunikation
zwischen der SPS und dem Roboter behandelt. Es beschreibt das Einrichten der
schnittstelle mit den durchgefhrten Tests.
c) Das Kapitel 4 behandelt die mathematischen Lsungsanstze. Zunchst werden
die fr die Berechnung notwendigen Grundlagen erlutert. Dabei wird
Schrittweise vorgegangen:
- Transformation der Koordinaten vom Greiferkoordinatensystem ins
Basiskoordinatensystem und Test
- Vorwrtstransformation zur Ermittlung der Raumkoordinaten aus den
Gelenkwinkelstellungen und Test
- Differentielle Vorwrtstransformation ( Jakobimatrix ) mit Linearisierung im
Arbeitspunkt
- Rckwrtstransformation: Invertierung der Jakobimatrix zur Bestimmung der
Gelenkwinkelstellungen aus den Raumkoordinaten
d) Kapitel 5 beschftigt sich mit dem alternativen Lsungsweg, der seinen Ursprung
in der Trigonometrie hat.
e) Das Kapitel 6 beschreibt die erstellte Visualisierung und erlutert die Bedienung
des Roboters.
f) Kapitel 7 enthlt das erstellte Anwenderprogramm mit Beschreibung. Zur
Programmierung wurde hauptschlich die IEC-Normsprache ST ( strukturierter
Text ) verwendet: ST stellt die notwendigen trigonometrischen Funktionen zur
Verfgung. Des Weiteren lassen sich die komplexen Formeln leicht realisieren.
g) Das Kapitel 8 soll ein Fazit ber die Arbeit liefern und weiterhin einen Ausblick
verschaffen auf Projekte, die auf dieser Studienarbeit aufbauen
( z.B. Werterfassung ber eine Bildverarbeitung ).
Studienarbeit Ansteuerung Grundlagen
eines Roboterarms
2 Grundlagen
2.1 Aufbau eines Industrieroboters
Kern eines Industrieroboters ist das kinetische Teilsystem, der sogenannte
Manipulator, der aus einer Folge von translatorischen und rotatorischen Gelenken
besteht und daher eine kinematische Kette darstellt. Der Manipulator kann auf einen
Fu oder aber bei mobilen Robotern auf ein Fahrzeug montiert sein. Der Effektor ist
ein Sammelbegriff fr ein am Ende des Manipulators angebrachtes Werkzeug oder
Greifer. Aufgabe des Manipulators ist es, den Effektor in die gewnschte Lage und
Orientierung innerhalb des Arbeitsraumes zu bringen. Welche Voraussetzungen
muss er dafr erfllen?
Jedes Objekt im Raum besitzt drei Freiheitsgrade der Position ( x, y, z ) sowie drei
Freiheitsgrade der Orientierung ( Drehung um die Winkel , , um die x, y, z
Achsen ). Die Position und Orientierung des Effektors wird bezglich eines im
Zentrum des Effektors gedachten Tool Center Point ( TCP ) angegeben. Um jede
beliebige Position und Orientierung des Effektors im Arbeitsraum einstellen zu
knnen, bentigt der Manipulator je Freiheitsgrad des Effektors ein translatorisches
oder rotatorisches Gelenk, also insgesamt sechs Gelenke. Dabei werden Gelenke,
die vorwiegend zur Positionierung des TCPs im Raum bentigt werden, als
Hauptachsen bezeichnet. Die Nebenachsen dienen im Gegensatz dazu zum
Einstellen der Orientierung des Effektors im angefahrenen Raumpunkt.
Ein solches System, das nur aus rotatorischen Gelenken besteht, ist beispielsweise
der menschliche Arm: Er besitzt vier Hauptachsen, nmlich das Ellenbogengelenk
und das Kugelgelenk der Schulter mit drei Freiheitsgraden und somit also eine Achse
mehr als mindestens erforderlich. Die Bewegungsmglichkeiten des Handgelenkes
stellen die Nebenachsen dar, nmlich das Kippen der Hand zur Seite und nach vorne
sowie das Drehen um die Achse des Unterarms. Die Hand selbst kann
als ein hchst flexibler Effektor mit zahlreichen Sensoren interpretiert werden.
Als Antrieb der Gelenke bei mechanischen Robotern werden in der Regel
Gleichstrommotoren mit Getriebe eingesetzt. Bei groen Lasten finden
Drehstrommotoren Anwendung. Ist eine hohe Positioniergenauigkeit erforderlich,
werden Schrittmotoren verwendet.
eines Roboterarms
2.2 Kinematische Beschreibung des Manipulators
Klassifikation:
Industrieroboter unterscheiden sich in Art und Abfolge ihrer Gelenke. Abbildung 2.1
zeigt exemplarisch die schematische Darstellung der kinematischen Kette eines 2
achsigen Roboters. Abbildung 2.2 erlutert die Bedeutung der einzelnen Elemente
aus Abbildung 2.1. Fr eine Klassifikation der Robotertypen ist die Kinetik der
Hauptachsen entscheidend.
Abbildung 2.1: schematische Darstellung
einer kinematischen Kette
Bezeichnung Symbol
Translationsachsen
Rotationsachsen
Werkzeuge/ Greifer
Armglied
Abbildung 2.2: Bedeutung der Elemente aus Abbildung 2.1
eines Roboterarms
2.3 Aufbau des Roboterarms ROB 3
Wie in Abbildung 2.3 dargestellt besteht der Roboterarm ROB 3 aus insgesamt 6
Achsen. An der letzten Achse befindet sich als Werkzeug ein Greifer, der
sogenannte Endeffektor, der geffnet und geschlossen werden kann. Die Bauweise
des Roboterarms erlaubt die in Abbildung 2.4 dargestellten Winkelbereiche.
Abbildung 2.3: Achsen des Roboters ROB 3
Gelenk Achse Winkelbereiche Schrittweite
(in Inkrementen)
Achse 1 - Krperdrehung q0 +80..0..-80 0255
Achse 2 - Schulter______ q1 +70..0..-30 0255
Achse 3 - Ellbogen_____ q2 0..-100 0255
Achse 4 - Handgelenk___ q3 +100..0..-100 0255
Achse 5 - Handdrehung_ q4 +100..0..-100 0255
Greifer - 0.60mm 0255
Abbildung 2.4: Winkelbereiche des Roboters ROB 3
Achse 2
Achse1
Achse 3
Greifer
Achse 4
Achse 5
eines Roboterarms
2.4 Arbeitsraum des Roboterarms ROB 3
In Abbildung 2.5 [ Quelle 13, Seite 17 ] wird der Arbeitsraum ( engl. working range )
des Roboterarms beschrieben. Liegt ein zu greifendes Objekt auerhalb dieses
Bereiches, kann es nicht erreicht werden. Raumpunkte, die der Roboter mit seinem
Greifer erreichen kann, werden mit dem Tool Center Point ( TCP ) beschrieben.
Abbildung 2.5: Arbeitsraum des Roboters ROB 3
TCP
Draufsicht: Seitenansicht:
Studienarbeit Ansteuerung serielle Kommunikation
eines Roboterarms
3 Serielle Kommunikation zwischen
Steuerung und Roboterarm
Der Roboterarm Rob3 soll mit einer Steuerung kommunizieren, die in Form einer
Soft-SPS auf einem Standard-PC abluft ( siehe Abbildung 3.1 ). Die Integration der
Soft-SPS in das Betriebssystem geschieht ber proprietre Microsoft-Techniken wie
DDE, OLE, etc. Dabei wird eine einfache Verbindung zu anderen Windows-
Applikationen oder C-Anwendungen ber OCX- bzw. ActiveX-Objekte ermglicht.
Die Verbindung zum Roboter erfolgt ber die RS 232Schnittstelle des PCs. Das
Kapitel 3 beschreibt den Aufbau dieser Kommunikationsverbindung und die in
diesem Zusammenhang verwendeten Befehlsbausteine. Eine Kurzbeschreibung der
Entwicklungsumgebung befindet sich in Anhang A.
Abbildung 3.1: Verbindung zwischen PC und Roboter
- Software CoDeSys: Ablaufsteuerung
- Kommunikation ber serielle Schnittstelle
- Visualisierung
- Low - Level Protokoll
Standard-PC:
Roboter:
- Controller
RS 232:
- Steuerbefehle und
Sollwerte an Roboter
- Istwerte an SPS
eines Roboterarms
3.1 Beschreibung der Schnittstellenbausteine
von CoDeSys
Die Entwicklungsumgebung CoDeSys erlaubt es dem Anwender, ber spezielle
Schnittstellenbausteine auf die Standard-Ports des PCs zugreifen zu knnen.
Die folgende Beschreibung bezieht sich auf die Bausteine der Version 2.3, die
allerdings nicht in lteren Versionen von CoDeSys verwendbar sind.
Um eine serielle Kommunikation zwischen SPS und einem Zielrechner aufbauen zu
knnen mu die Bibliothek SysLibCom.lib in das Projekt eingebunden werden. Die
Bibliothek stellt folgende Funktionen zur Verfgung, die das ffnen und Schlieen
eines seriellen Ports ermglichen. ber eine geffnete Verbindung knnen dann
Daten geschrieben und gelesen werden.
Anmerkung:
Der folgende Abschnitt liefert einen kurzen berblick ber Bausteine der Bibliothek
SysLibCom.lib. Eine Beschreibung der Bausteine im Anwendungsfall erfolgt in
Kapitel 6.
a) Funktion SYSCOMSETTINGS:
Die Funktion SysComSettings dient dazu, die Parameter eines seriellen Port wie
Baudrate, Stoppbits, Parittsbits, Funktions-Timeout, Buffergre und Scan-Time zu
setzen. Dieser Datensatz ist in der Struktur ComSettings zusammengefasst.
b) Funktion SYSCOMOPEN:
Durch Aufruf der Funktion SYSCOMOPEN lsst sich ein serieller Port ffnen. Als
Rckgabewert erhlt die Funktion einen Handle, der den geffneten Port definiert.
Dieser Handle wird bei Aufruf der Funktion SysComSettings bergeben.
Falls das ffnen des Ports nicht mglich ist, wird der Wert 16#FFFFFFFF
( =10#4294967295 ) zurckgeliefert.
eines Roboterarms
c) Funktion SYSCOMREAD:
Die Funktion SYSCOMREAD vom Typ DWORD liest Daten von dem durch den
Handle definierten Port. Die Eingangsvariablen dieser Funktion sind:
- DwHandle: der mit SysComOpen erhaltene Port-Handle
- DwBufferAddress: Adresse, auf welche die gelesenen Daten kopiert werden
sollen
- DwBytesToRead: die Anzahl der erwarteten Bytes
- DwTimeout: Timeout-Zeit der Funktion
Als Rckgabewert liefert die Funktion die Anzahl der tatschlich gelesenen Bytes.
d) Funktion SYSCOMWRITE:
Die Funktion SYSCOMWRITE vom Typ DWORD schreibt die Daten an den Port,
der durch den erhaltenen Handle aus SysComOpen definiert wird. Die
Eingangsvariablen dieser Funktion sind:
- DwHandle: der mit SysComOpen erhaltene Port-Handle
- DwBufferAddress: Adresse, aus der die Daten geschrieben werden sollen
- DwBytesToWrite: Gre des zu schreibenden Datenblocks in Byte
- DwTimeout: Timeout der Funktion
Als Rckgabewert wird die Anzahl der tatschlich geschriebenen Bytes geliefert.
e) Funktion SYSCOMCLOSE:
Die Funktion SYSCOMCLOSE vom Typ BOOL schliet den COM-Port, dazu ist der
bei SysComOpen enthaltene Port-Handle zu bergeben. Als Rckgabewert erhlt
man entweder FALSE oder TRUE. Dieser Wert gibt Auskunft ber Erfolg der
Funktion.
eines Roboterarms
3.2 Beschreibung des Datenaustauschprotokolls
Das von der Firma Eurobtec verwendete Protokoll wird als LowLevelProtokoll
bezeichnet. ber dieses Protokoll lsst sich der Roboterarm ber die SPS ansteuern.
Der Controller im Roboterarm empfngt Steuerdaten und regelt die Achsstellungen
auf die empfangenen Sollwerte. Bei Bedarf sendet er Daten zurck ( z.B. Istwerte ).
Die Kommunikation luft ohne Hardware-Handshake ab.
3.2.1 Kommunikationsaufbau
Um eine Kommunikation zum Roboter aufbauen zu knnen, mu zuerst der Baustein
SYSCOMOPEN aufgerufen werden, um den bertragungskanal zu ffnen. Bei
erfolgreicher Etablierung einer Verbindung wird ein Steuersatz von zwei Byte an den
Controller gesendet. Das erste Byte enthlt den Befehl 16#20 zur Initialisierung des
Controllers. Das zweite Byte dient zum Abschlu der Anweisung mit der Information
ETX ( 16#03 ). Als Antwort signalisiert der Roboterarm eine fehlerfreie
Startinitialisierung mit den Zeichen 16#15, 16#F1,16#F2 oder 16#F3.
Falls der Roboterarm andere Werte liefert, war der Kommunikationsaufbau nicht
erfolgreich. Eine Auswertung erfolgt im Anwenderprogramm. Bei erfolgreicher
Initialisierung wird mit 16#61 und ETX die Regelung des Controllers eingeschaltet.
3.2.2 Senden der Steuerdaten
Bevor man Steuerdaten an den Roboterarm senden kann, mssen die Bedingungen
aus 3.2.1 erfllt sein. Ist dies der Fall, kann mit der bertragung von
Kommandostzen begonnen werden. Ein Kommandosatz besteht hierbei aus
maximal 8 Bytes, wobei das letzte Byte das Zeichen ETX enthlt, um den
Kommandosatz abzuschlieen. Der Aufbau eines Kommando-Telegramms wird im
Kapitel 6 in der Programmbeschreibung verdeutlicht.
eines Roboterarms
3.2.3 Ansteuerkommandos fr den Roboterarm
Die folgenden Daten sind aus dem Handbuch der Firma Robotec
entnommen [ Quelle 12, Seite 1-8 ]:
a) Positionier-Kommandos:
eine Achse | 0 | 0 | 0 | 0 | R | a | a | a | 00 0D Hex
mit R = 1: Rckmeldung nach dem Erreichen der Sollposition
mit a = Achsennummer (0-5, 5 entspricht dem Greifer)
alle Achsen | 0 | 0 | 0 | 0 | R | 1 | 1 | 1 | 0F Hex
Dem Kommando folgen sieben Byte mit den Sollwerten (0-255) und ETX.
Die Rckmeldung des Roboters ist das Kommandoschlsselwort und ETX.
b) Positionier Kommandos mit Geschwindigkeit:
eine Achse | 0 | 1 | 1 | 1 | R | a | a | a | 70 7D Hex
mit a = Achsennummer (0 5, 5 entspricht dem Greifer)
Dem Kommando folgt ein Byte mit dem Sollwert (0-255) und ein Byte mit dem
Zeitfaktor T: sinnvolle Werte fr den Zeitfaktor liegen zwischen 0 ( max.
Geschwindigkeit ) und 7 ( sehr langsam ). Mit der bernahme des Kommandos wird
die vorhergehende Sollposition jeweils nach Ablauf der Zeit T um einen Schritt erhht
bzw. erniedrigt bis die Endposition erreicht ist. Die Zeit T errechnet sich aus
Zeitfaktor*10 msec. Bei Zeitfaktor 0 wird der Roboter mit seiner maximalen
Geschwindigkeit angefahren. Zeitfaktor 7 bedeutet eine weitere nderung der
Achsenposition nach 70msec. Der Kommandosatz wird mit ETX abgeschlossen.
Die Rckmeldung des Roboters ist das Kommandoschlsselwort und ETX.
alle Achsen | 0 | 1 | 1 | 1 | R | 1 | 1 | 1 | 7F Hex
Es folgen sechs Byte Sollwerte (0-255), sechs Byte Zeitfaktoren T und ETX.
Kommando-Byte
eines Roboterarms
c) Regel Kontroll Kommandos:
- 60 Hex : Motor Regelung ausschalten
- 61 Hex : Motor Regelung einschalten
- 62 Hex : Positionier Abschaltung (Software Not Aus)
momentane Ist Position bleibt erhalten
d) Seriennummer Abfrage:
- 63 Hex: Seriennummer Abfrage
Der Roboter antwortet daraufhin mit 16#63, 16#S0, 16#S1, 16#S2 und ETX.
e) Positions-Abfrage-Kommandos:
eine Achse | 0 | 1 | 0 | 0 | 0 | a | a | a | 40 45 Hex
mit a = Achsennummer (0 5)
Rckmeldung:
- Kommando
- ein Byte Istwert (0-255)
- ETX
alle Achsen | 0 | 1 | 0 | 0 | 0 | 1 | 1 | 1 | 4F Hex
Rckmeldung:
- Kommando
- 7 Byte Istwert (0-255)
- ETX
eines Roboterarms
3.3 Testen der Verbindung
3.3.1 Test der Ansteuerung ber die serielle Schnittstelle
Es scheint nicht sinnvoll, gleich die Kommunikation zwische SPS und Roboter zu
realisieren. So erfolgt das Einrichten und das Testen der RS232-Verbindung Schritt
fr Schritt. Zunchst wird dabei die generelle Kommunikationfhigkeit der
Schnittstelle geprft. Dann wird der Kommunikationsaufbau zwischen der Runtime
und einem zweiten Rechner realisiert, um sich schlielich an die Lsung
heranzuarbeiten ( Runtime Roboter ).
Vorgehensweise:
a) Verbindung PC PC:
Die Verbindung der beiden PCs erfolgt ber Nullmodem-Kabel. Auf beiden Rechner
wird das Hyperterminal gestartet ( Start\Programme\Kommunikation\Hyperterminal )
und konfiguriert fr COM2 mit einer Baudrate von 9600. Mit dem Befehl Anrufen
wird die Verbindung geffnet.
Ergebnis: Ein auf der Tastatur des einen Rechners eingegebenes Zeichen wird im
Anzeigefenster des Hyperterminals auf dem Kommunikationspartner
angezeigt.
b) Verbindung Runtime PC:
Nach erfolgreichem Test der Verbindung aus a) wird nun auf einem der Rechner (
ber den spter die Steuerung erfolgen soll ) das Hyperterminal durch die Runtime
der Soft-SPS ersetzt. Dazu mu die Runtime unter D:\Programme\3SSoftware\
CoDeSys SP RTE\RTService.exe gestartet werden. Ein mit CoDeSys entwickeltes
Testprogramm soll dann den Datenaustausch steuern. Das Programm enthlt die
Funktionen der Bibliothek SysComLib.lib. Auf dem Zielrechner wird das
Hyperterminal geffnet und konfiguriert.
Ergebnis: Nach dem Anrufen der Verbindung auf dem Zielrechner, dem Start der
Runtime auf dem Steuerrechner und dem Aufruf des Testprogramms in
der Runtime kann der Kommunikationskanal ber die
Kommunikationbausteine geffnet werden.
In einer ersten Version des Testprogramms sendet die SPS
kontinuierlich beliebige ASCII-Zeichen an den Zielrechner. Diese
werden im Anzeigefenster des Hyperterminals ausgegeben.
Das zweite Testprogramm liest die Tastatureingabe vom Zielrechner
und sendet sie im nchsten Zyklus zur Ausgabe im Anzeigefenster des
Hyperterminals zurck.
eines Roboterarms
c) Verbindung Runtime Roboter:
Nach dem Test der Verbindung aus b) wird nun der Rechner, auf dem das
Hyperterminal geffnet war, durch den Roboter ersetzt. Fr die Verbindung ist ein
gewhnliches Verbindungskabel ntig. Das Testprogramm aus b) kann fr die
Ansteuerung des Roboters angepat werden.
Ergebnis: Im angepaten Testprogramm kann im Online-Betrieb der
bertragungskanal durch Setzen entsprechender Bits ( bopen und
bsend ) geffnet werden. Nach dem ffnen und erfolgreicher
Initialisierung des Controllers knnen die beschriebenen
Kommandostze bertragen werden.
3.3.2 Aufgetretene Probleme beim Test der seriellen
Kommunikation
a) Aufbau der Verbindung:
Mit CoDeSys Version 2.2 ist der Aufbau einer Verbindung zu einem Partner nur
mglich, wenn gleichzeitig das Hyper Terminal auf dem selben Rechner geffnet ist.
Aus diesem Grund wird CoDeSys Version 2.3 auf dem System installiert.
Vor dem Einspielen des neuen Softwarestandes mssen alle Komponenten von
Version 2.2 vollstndig entfernt werden.
b) Anpassen des Ansteuerprogramms:
Mit der Umstellung auf Version 2.3 mu auch das Ansteuerprogramm angepat
werden, da sich die Bausteine der Bibliothek SysLibCom.lib verndert haben.
c) Initialisierung des Roboters:
Es kann vorkommen, dass die Initialisierung des Roboters fehlschlgt. In diesem Fall
ist der Reset-Button am Roboter zu bettigen. Anschlieend hat ein neuer
Verbindungaufbau zu erfolgen.
In einigen Fllen reicht es, bei fehlgeschlagener Initialisierung kurz den
Anschlussstecker am Roboter zu ziehen und ihn dann wieder aufzustecken. Der
Roboter wird dabei initialisiert
d) Starten der Runtime:
Ein Start der Runtime der Version 2.3 ist nur mit Administratorrechten mglich.
eines Roboterarms
3.3.3 Probleme beim Test der Achsansteuerung
a) Problem:
Einige Gelenke des Roboters sind nicht abgeglichen, d.h. ihre tatschliche Position
entspricht nicht der angegebenen Istposition. Insbesondere bei Gelenk 3 fhrt diese
Tatsache zu Fehlern. So ist das Armglied mechanisch bereits am Anschlag, die
Regelung im Controller hat die Sollposition aber noch nicht erreicht. Daher wird
versucht den Sollwert einzustellen und der Antrieb luft weiter gegen den Anschlag.
Konsequenz ist ein berstrom, der zur Abschaltung des Roboters fhrt. In diesem
Fall ist eine Neuinitialisierung ntig.
b) Justierung von Gelenk 3:
Das Potentiometer zur Sollwerterfassung befindet sich am Antrieb und ist schwer zu
erreichen. Der Antrieb bewegt das Gelenk ber Zahnriemen. Somit erfolgt die
Messung der Winkelstellung nicht direkt am Gelenk. Zur Justierung msste der
ganze Arm zerlegt werden. Daher wird wie folgt vorgegangen:
- Das Antriebszahnrad auf der Gelenkachse wird ber zwei Imbusschrauben
gelst.
- Der Antrieb wird ber die Regelung auf Endposition gefahren. Da das
Armglied abgekoppelt wurde, bewegt es sich nicht mit.
- Das Armglied kann nun manuell an die Sollposition gebracht werden.
- Die Imbusschrauben werden angezogen.
- Software-Endposition und mechanische Endposition stimmen wieder berein.
c) Justierung der Gelenke 1 und 2:
Bei den Gelenken 1 und 2 befinden sich die Istwertpotis auf der Auenseite der
Armglieder direkt an der Drehachse. Die Potis knnen an den
Befestigungsschrauben gelst und direkt verdreht werden. Bei eingeschalteter
Regelung verndert sich beim Drehen des Potis die Stellung des Armgliedes. In der
gewnschten Stellung werden die Befestigungsschrauben wieder angezogen.
d) Justierung von Gelenk 4:
Die Ansteuerung des Gelenkes erfolgt hier ber eine Zahnrad-bersetzung. Zur
Justierung von Gelenk 4 mu der Antrieb mit Zahnrad demontiert werden. Das
Zahnrad am Armgelenk kann dann im Bezug zum Antriebszahnrad manuell
verschoben werden. Nach Befestigung des Antriebs ist das Gelenk abgeglichen.
Studienarbeit Ansteuerung Kinematik
eines Roboterarms
4 Kinematik der Position
4.1 Einleitung
Das Kapitel 4 beschftigt sich mit der Roboter-Geometrie und den zeitabhngigen
Aspekten der Bewegung, ohne dabei die Krfte zu bercksichtigen, die diese
Bewegung verursachen. Die bedeutendsten Aspekte der Kinematik sind Position,
Geschwindigkeit, Verschiebung , Beschleunigung und Zeit.
In diesem Kapitel wird dargestellt, wie mit einer 4x4 Matrix Koordinatensysteme
durch eine homogene Transformation zueinander in Beziehung gesetzt werden.
Dadurch lassen sich Daten eines Endeffektor-Koordinatensystems in ein gewhltes
Basiskoordinatensystem bertragen. Darauf aufbauend kann mit der
Matrizenrechnung vom Gelenkraum in den kartesischen Raum transformiert werden:
Aus den als bekannt vorausgesetzten Gelenkvariablen werden Position und
Orientierung des Greifer berechnet.
4.2 Grundlagen der Kinematik
4.2.1 Einfhrung in die zugrundeliegende Matrizenrechnung
a) Multiplikation von Matrizen:
[ Quelle 1, Seite 27 / Quelle 6, Seite 193ff ]
A= (a
ik
) sei eine Matrix vom Typ (m, p), B =(b
ik
) eine Matrix vom Typ (n, p).
Dann heit die(m, p)- Matrix C = A * B = (c
ik
) mit
Das Produkt der Matrizen A und B (i=1,2,.,m, k = 1,2,.p),
Anmerkungen:
Die Produktbildung ist nur mglich, wenn die Spaltenzahl von A mit der Zeilenzahl
von B bereinstimmt. Der Multiplikationspunkt darf auch weggelassen werden.
Das Matrixelement (c
ik
) des Matrizenproduktes A * B ist das Skalarprodukt aus dem i
- ten Zeilenvektor von A und dem k-ten Spaltenvektor von B ( siehe Falk-Schema
weiter unten).
(c
ik
) = a
i1
b
1k
+ a
i2
b
2k
+ ... + a
in
b
nk
=

=
n
j 1
a
ij
b
jk
eines Roboterarms
- Falk-Schema zur Berechnung eines Matrizenproduktes C = A * B:
(c
ik
): Skalarprodukt aus dem i-ten Zeilenvektor von
A und dem k-ten spaltenvektor von B
- Rechenregeln:
Voraussetzungen: alle Rechenoperationen der linken Seite mssen
durchfhrbar sein.
Assoziativgesetz: A(BC) = (AB)C
Distributivgesetz: A(B+C) = AB + BC
(A+B)C =AC + BC
Transponieren: (AB)
T
= B
T
A
T
Man beachte, da die Matrixmultiplikation nicht kommutativ ist
A*B B*A
b) Inverse Matrix:
- Definition einer inversen Matrix:
Die regulren Matrizen (und nur diese) lassen sich umkehren, d.h. zu jeder
regulren Matrix A gibt es genau eine inverse Matrix A
-1
mit
Eine quadratische Matrix ist demnach genau invertierbar, wenn det A ungleich
0 und somit Rg(A) = n ist.
Weitere Bezeichnungen fr A
-1
: Kehrmatrix, Umkehrmatrix
oder Inverse Matrix von A
Einheitsmatrix E: Diagonalmatrix mit a
ii
= 1 und a
ij
= 0
A*A
-1
= A
-1
*A = E
A
A * B B
Matrix A: Typ (m,n)
Matrix B: Typ (m,n)
k-te Spalte
i-te
zeile
eines Roboterarms
- Berechnungen einer inversen Matrix:
Berechnungen der inversen Matrix A
-1
unter Verwendung von
Unterdeterminaten
A
ik
: Algebrasches Komplement ( Adjunkte ) von a
ik
in detA (A
ik
= (-1
)i+k
* D
ik
)
D
ik
: (n-1)- reihige Unterdeterminate von detA
(in detA wird die i-te Zeile und k-te Spalte gestrichen)
4.2.2 Regeln fr die Festlegung der Koordinatensysteme
Zur rumlichen Darstellung von Koordinatensystemen wird die in Abbildung 4.1
dargestellte Rechte Hand-Regel verwendet. Die Y-Acchse zeigt dabei senkrecht
nach oben, die Z-Achse aus der Bildebene heraus und Die X-Achse zeigt nach
rechts. Die Festlegung der Koordinatensysteme in den Achsen des Roboterarms
basiert ebenfalls auf der Rechte-Hand- Regel.
Abbildung 4.1: Rechte-Hand- Regel
A
-1
=
A det
1
nn n
n
n
A A A
A A A
A A A
...
: : : :
...
...
4 41
2 22 21
1 12 11
; (det A ungleich 0)
eines Roboterarms
a) Definition Rotation:
Bei einer Rotation wird die Z-Achse in die Rotationsachse gelegt ( siehe Abbildung
4.2 ). X-Achse und Y-Achse werden beliebig nach der Rechte-Hand-Regel
festgelegt.
Abbildung 4.2: Festlegung des Koordinatenystems
In einem rotatorischen Gelenk
b) Definition Translation:
Bei einer Translation wird die Z-Achse in die Rotationsachse gelegt ( siehe Abbildung
4.3a ). Die X-Achse wird als Verlngerung des Armgliedes l
n
festgelegt. Die Y-Achse
ergibt sich aus der Rechte-Hand-Regel ( siehe Abbildung 4.3b ).
Abbildung 4.3a/b: Festlegung des Koordinatensystems
In einem translatorischen Gelenk
Sollten ein rotatorisches und ein translatorisches Gelenk berlagert sein, wird wie
folgt verfahren:
Die beiden Gelenke werden getrennt dargestellt und durch ein virtuelles Armglied der
Lnge 0 verbunden.
Z
n
Y
n
X
n
z
n
l
n+1
l
n
z
n
y
n
x
n
l
n+1
l
n
a) b)
eines Roboterarms
4.2.3 Festlegung der Koordinatensystem3 fr
den Roboterarm ROB3
Abbildung 4.4 zeigt die nach den genannten Regeln gewhlten Koordinatensysteme
des Roboterarms. Diese Festlegung ist Ausgangspunkt fr die sptere
Matrizenberechnung.
Das Koordinatensystem x1,y1,z1 liegt im Gelenk 1 und ist gegenber dem
vorangegangenen Basiskoordinatensystem in Gelenk 0 um a1 verschoben. Der
Winkel Q0 drckt die Verdrehung des Koordinatensystems 1 um die Z-Achse des
vorangegangenen Koordinatensystems aus. Die Bezeichnungen der folgenden
Gelenke 2 bis 5 wurden auf analoge Weise festgelegt.
Abbildung 4.4: Festlegung der Koordinatensysteme
des Roboters ROB 3
y
0
z
0
x
1
q
1
y
1
z
1
q
3
q
2
x
2
y
2
z
4
x
4
y
4
y
3
z
2
z
3
l
1
l
2
l
3
l
5
l
0
q
4
l
4
= 0;
x
0
q
0
x
3
x
4
z
5
x
5
y
5
q
4
a1 = l
1
+ l
0
eines Roboterarms
4.2.4 Einfhrung in die notwendige Vektorrechnung
Diese Einfhrung soll einen berblick ber die bentigten Operationen der
Vektorrechnung liefern. Grundkenntnisse der Vektorrechnung werden hierbei als
bekannt vorausgesetzt. Grundlagen der Matrizenrechnung werden bereits im Kapitel
4.2.1 behandelt.
a) Darstellung eines Raumpunktes:
Ein Punktvektor verbindet das Basiskoordinatensystem mit einen Punkt und stellt
somit dessen Koordinaten dar. Ein Punktvektor kann im dreidimensionalen Raum auf
folgende Weise realisiert werden:
(
(
(
(
w
z
y
x
X stellt dabei die Koordinate in X-Richtung, y die Koordinate in Y-Richtung und z die
Koordinate in Z-Richtung dar. W wird konstant mit 1 belegt und hat fr die
Raumposition direkt keine Bedeutung. W wird allerdings bentigt, um Koordinaten
von einem Koordinatensystem in ein anderes transformieren zu knnen.
Ein Vektor dieser Art wird verwendet, um die Position des TCPs innerhalb eines
Koordinatensystems ausdrcken zu knnen.
b) Umrechnung in ein anderes Koordinatensystem:
Um einen Punkt P eines Koordinatensystems in einem anderen Koordinatensystem
darstellen zu knnen, mu der zugehrige Punktvektor P mit einer entsprechenden
Transformationsmatrix H multipliziert werden. Das Ergebnis dieser Operation ist der
Punktvektor P des Punktes im neuen Koordinatensystem.
homogene Transformation: P = H * P
Die Transformationsmatrix setzt die beiden Koordinatensysteme in Beziehung
zueinander.
eines Roboterarms
c) Transformationsmatrix fr verschobene Koordinatensysteme:
Ist der Ursprung des zu transformierenden Koordinatensystems gegenber dem
neuen Kordinatensystem um die Werte x in XRichtung,y in YRichtung und
z in ZRichtung verschoben, so lautet die Transformationsmatrix H:
H = Trans (x,y,z) =
(
(
(
(
1 0 0 0
1 0 0
0 1 0
0 0 1
z
x
x
;
d) Tranformationsmatrix fr verdrehte Koordinatensysteme:
Ist das ursprngliche Koordinatensystem um den Winkel q um die X-Achse des
neuen Koordinatensystem gedreht, so lautet die Transformationsmatrix:
Rot(x, q) =
(
(
(
(
1 0 0 0
0 cos sin 0
0 sin cos 0
0 0 0 1
q q
q q
;
Fr die Verdrehung q um die Y-Achse lautet die Transformation:
Rot(y, q) =
(
(
(
(
1 0 0 0
0 cos 0 sin
0 0 1 0
0 sin 0 cos
q q
q q
Fr die Verdrehung q um die ZAchse lautet die Transformation:
Rot(z, q) =
(
(
(
(

1 0 0 0
0 1 0 0
0 0 cos sin
0 0 sin cos
q q
q q
;
eines Roboterarms
4.3 Bestimmung der absoluten Position und Orientierung
des TCP im Basiskoordinatesystem
[ Quelle 4, Seite 147-148 ]
Unter Anwendung der erluterten Operationen der Matrizen- und Vektorrechnung
lt sich nun fr jedes Achsgelenk eine Transformationsmatrix aufstellen. Diese
homogene Transformation setzt das im Gelenk liegende Koordinatensystem zum
Koordinatensystem des benachbarten Gelenks bezglich Verdrehung und
Verschiebung in Beziehung. Durch Kombination aller homogenen Transformationen
kann schlielich der TCP vom Greiferkoordinatensystem in das
Basiskoordinatensystem berfhrt werden. Dazu mssen alle Teilmatrizen in der
richtigen Reihenfolge miteinander multipliziert werden:
R
T
H
= A
0
.
A
1
.
A
2
.
A
3
.
A
4
.
A
5
- Die Multiplikation erfolgt der Reihenfolge nach von rechts nach links
- Die Indizes 0 bis 5 bezeichnen die Gelenknummern
-
R
T
H
beschreibt eine 4x4 Transformationsmatrix, die den TCP im
Basiskoordinatensystem ausdrckt
4.3.1 Berechnung der A-Matrizen
Der Roboter ROB 3 der Firma EUROBTEC verfgt ber fnf Freiheitsgrade ( q
0
bis
q
4
). Das sechste Gelenk, der Greifer, hat keinen Einfluss, auf
R
T
H
. Demnach sind 5
Teilmatrizen A aufzustellen. Die Vorgehensweise wird im folgenden Abschnitt
erlutert.
a) Berechnung der Matrix
1
A
0
:
Um das erste Koordinatensystem in das nullte zu berfhren, mu es in folgender
Reihenfolge rotiert bzw. verschoben werden :
1
A
0
: Rot( z, 90+ q
0
); Rot( y, 270); Trans( 0, 0, l
1
)
- Z-Achse: Rotation um 90+ q
0
;
- Y-Achse: Rotation um 270;
- Translation: x = 0; y = 0; z: = l
0
+l
1
;
Die Reihenfolge der Transformationen ist strikt einzuhalten.
eines Roboterarms
Daraus ergeben sich die Transformationsmatrizen:
-
(
(
(
(
+
+ +
1 0 0 0
0 ) cos( ) 90 sin( 0
0 ) 90 sin( ) 90 cos( 0
0 0 0 1
0 0
0 0
q q
q q
=
(
(
(
(
+
+ +
1 0 0 0
0 ) cos( ) 90 sin( 0
0 ) 90 sin( ) 90 cos( 0
0 0 0 1
0 0
0 0
q q
q q
-
(
(
(
(
+

1 0 0 0
0 ) 270 cos( 0 ) 90 sin( ) 270 sin(
0 0 1 0
0 ) 270 sin( 0 ) 270 cos(
0 q
=
(
(
(
(

1 0 0 0
0 0 0 1
0 0 1 0
0 1 0 0
Die Multiplikation der beiden Matrizen ergibt:
(
(
(
(
+
+ +
1 0 0 0
0 ) cos( ) 90 sin( 0
0 ) 90 sin( ) 90 cos( 0
0 0 0 1
0 0
0 0
q q
q q
(
(
(
(

1 0 0 0
0 0 0 1
0 0 1 0
0 1 0 0
(
(
(
(
1 0 0 0
0 0 0 1
0 ) cos( ) sin( 0
0 ) sin( ) cos( 0
0 0
0 0
q q
q q
Fgt man die Translation der Multiplikation hinzu so ergibt sich die Matrix
1
A
0
:
1
A
0
=
(
(
(
(
1 0 0 0
l1 lo 0 0 1
0 ) cos( ) sin( 0
0 ) sin( ) cos( 0
0 0
0 0
q q
q q
Gleichung 4.1
eines Roboterarms
b) Berechnung der Matrix
2
A
1
:
2
A
1
: Rot( z, -q
1
) ; Trans(l
2
*sin( 90+q
1
), -l
2
*cos( 90+q
1
), 0 )
- Z-Achse: Rotation um -q
1
;
- Translation: x= l
2
*sin( 90+q
1
); y.= -l
2
*cos( 90+q
1
); z = 0;
Daraus ergeben sich die Transformationsmatrizen:
-
(
(
(
(

1 0 0 0
0 1 0 0
0 0 ) cos( ) sin(
0 0 ) sin( ) cos(
1 1
1 1
q q
q q
=
(
(
(
(
1 0 0 0
0 1 0 0
0 0 ) cos( ) sin(
0 0 ) sin( ) cos(
1 1
1 1
q q
q q
-
(
(
(
(
1 0 0 0
0 1 0 0
) q - cos(90 * l - 0 1 0
) q - sin(90 * l 0 0 1
1 2
1 2
=
(
(
(
(
1 0 0 0
0 1 0 0
) q sin( * l - 0 1 0
) q ( cos * l 0 0 1
1 2
1 2
(
(
(
(
1 0 0 0
0 1 0 0
0 0 ) cos( ) sin(
0 0 ) sin( ) cos(
1 1
1 1
q q
q q
(
(
(
(
1 0 0 0
0 1 0 0
) q sin( * l 0 1 0
) q ( cos * l 0 0 1
1 2
1 2
(
(
(
(
1 0 0 0
0 1 0 0
) q sin( * l - 0 ) cos( ) sin(
) q ( cos * l 0 ) sin( ) cos(
1 2 1 1
1 2 1 1
q q
q q
Die Matrix
2
A
1
lautet:
2
A
1
=
(
(
(
(
1 0 0 0
0 1 0 0
) q sin( * l - 0 ) cos( ) sin(
) q ( cos * l 0 ) sin( ) cos(
1 2 1 1
1 2 1 1
q q
q q
Gleichung 4.2
eines Roboterarms
c) Berechnung der Matrix
3
A
2
:
3
A
2
: Rot( z, -q
2
) ; Trans( l
3
*sin( 90+q
2
), -l
3
*cos( 90+q
2
), 0 )
2
;
- Translation: x = l
3
*sin( 90+q
2
); y.= -l
3
*cos( 90+q
2
); z = 0;
Die Transformationen entsprechen denen der Matrix
2
A
1
:
3
A
2
=
(
(
(
(
1 0 0 0
0 1 0 0
) q sin( * l - 0 ) cos( ) sin(
) q ( cos * l 0 ) sin( ) cos(
2 3 2 2
2 3 2 2
q q
q q
Gleichung 4.3
d) Berechnung der Matrix
4
A
3
:
4
A
3
: Rot( z, -q
3
); Rot( y, -90); Trans(l
4
*cos( q
3
), l
4*
sin(q
3
), 0 )
3
;
- Y-Achse: Rotation um -90;
- Translation: x:=l
4
*cos( q
3
); y = l
4*
sin(q
3
); z = 0;
Achtung: Die Lnge l
4
ist Null ! Deshalb entfllt der Ausdruck.
-
(
(
(
(

1 0 0 0
0 ) 90 cos( 0 ) 90 sin(
0 0 1 0
0 ) 90 sin( 0 ) 90 cos(
3 3
3 3
q q
q q
=
(
(
(
(
1 0 0 0
0 ) sin( 0 ) cos(
0 0 1 0
0 ) cos( 0 ) sin(
3 3
3 3
q q
q q
-
(
(
(
(
+
+ +
1 0 0 0
0 ) cos( ) 90 sin( 0
0 ) 90 sin( ) 90 cos( 0
0 0 0 1
0 0
0 0
q q
q q
=
(
(
(
(
1 0 0 0
0 0 1 0
0 1 0 0
0 0 0 1
eines Roboterarms
(
(
(
(
1 0 0 0
0 ) sin( 0 ) cos(
0 0 1 0
0 ) cos( 0 ) sin(
3 3
3 3
q q
q q
(
(
(
(
1 0 0 0
0 0 1 0
0 1 0 0
0 0 0 1
(
(
(
(
1 0 0 0
0 0 1 0
0 ) sin( 0 ) cos(
0 ) cos( 0 ) sin(
3 3
3 3
q q
q q
Die Matrix
4
A
3
lautet:
4
A
3
=
(
(
(
(
1 0 0 0
0 0 1 0
0 ) sin( 0 ) cos(
0 ) cos( 0 ) sin(
3 3
3 3
q q
q q
Gleichung 4.4
e) Berechnung der Matrix
5
A
4
:
5
A
4
: Rot( z, q
4
); Trans( 0, 0, l
5
)
- Z-Achse: Rotation um q
4
;
- Translation: x = 0; y = 0; z = l
5
;
5
A
4
=
(
(
(
(

1 0 0 0
1 0 0
0 0 ) cos( ) sin(
0 0 ) sin( ) cos(
5
4 4
4 4
l
q q
q q
Gleichung 4.5
eines Roboterarms
4.3.2 Berechnung der Endmatrix
R
T
H
aus den A-Matrizen
Nach dem Aufstellen der A-Matrizen erfolgt die Zusammenfassung zur Endmatrix
R
T
H.
Die Multiplikation erfolgt unter Anwendung des Assoziativgesetzes. Die A-
Matrizen werden dabei geeignet zusammengefat, um die sich ergebenden
Ausdrcke einigermaen berschaubar zu halten.
a) Multplikation der Matrix
5
A
4
mit der Matrix
4
A
3
:
5
A
3
=
4
A
3
*
5
A
4
( von rechts nach links )
(
(
(
(

1 0 0 0
1 0 0
0 0 ) cos( ) sin(
0 0 ) sin( ) cos(
5
4 4
4 4
l
q q
q q
(
(
(
(
1 0 0 0
0 0 1 0
0 ) sin( 0 ) cos(
0 ) cos( 0 ) sin(
3 3
3 3
q q
q q
Die Matrix
5
A
3
lautet:
5
A
3
=
(
(
(
(

+
+
1 0 0 0
0 0 ) cos( ) sin(
* ) sin( * ) sin( ) sin( 0 ) cos(
* ) cos( * ) cos( ) cos( ) sin( * ) sin( ) cos( * ) sin(
4 4
4 3 5 3 3 3
4 3 5 3 3 4 3 4 3
q q
l q l q q q
l q l q q q q q q
Gleichung 4.6
eines Roboterarms
b) Multplikation der Matrix
3
A
2
mit der Matrix
2
A
1
:
3
A
1
=
4
A
3
*
3
A
2
Die Matrix
3
A
1
lautet:
3
A
1
=
(
(
(
(
+
+ +
1 0 0 0
0 1 0 0
l * ) q ( sin * ) cos( * ) sin( - l * ) q ( cos * ) q ( s - 0 ) cos( * ) cos( ) sin( * ) sin( ) cos( * ) sin( ) cos( * ) sin(
l * ) q ( cos * ) sin( * ) sin( - l * ) q ( cos * ) q ( cos 0 ) cos( * ) sin( ) cos( * ) sin( ) sin( * ) sin( ) cos( * ) cos(
2 1 3 1 2 3 2 1 2 1 2 1 1 2 2 1
2 1 3 2 1 3 1 2 2 2 1 2 2 1 2 1
l q q in q q q q q q q q
l q q q q q q q q q q
Nach den Gesetzen der Winkelumformung ergibt sich der verkrzte Ausdruck:
3
A
1
=
(
(
(
(
+ + +
+ + + +
1 0 0 0
0 1 0 0
l * ) q ( sin l * ) sin( 0 ) cos( ) sin(
l * ) q ( cos l * ) cos( 0 ) sin( ) cos(
2 1 3 2 1 2 1 2 1
2 1 3 2 1 2 1 2 1
q q q q q q
q q q q q q
Gleichung 4.7
c) Multplikation der Matrix
3
A
1
mit der Matrix
1
A
0
:
3
A
0
=
1
A
0
*
3
A
1
Die Matrix
3
A
0
lautet:
3
A
0
=
Gleichung 4.8
(
(
(
(
+ + + + +
+ + + +
+ + + +
1 0 0 0
) q ( cos l * ) q ( cos l * ) cos( 0 ) sin( ) cos(
l * ) q ( sin * ) sin( l * ) sin( * ) sin( ) cos( ) cos( * ) sin( ) sin( * ) sin(
l * ) cos( ) q ( sin l * ) cos( * ) sin( ) sin( ) cos( * ) cos( ) cos( * ) sin(
1 2 1 3 2 1 2 1 2 1
2 1 0 3 2 1 0 0 2 1 0 0 2 1
2 0 1 3 0 2 1 0 0 2 1 0 2 1
q q q q q q
q q q q q q q q q q q
q q q q q q q q q q q
eines Roboterarms
d) Multplikation der Matrix
3
A
0
mit der Matrix
5
A
3
:
Aufgrund der Gre der Ausdrcke ist es hier nicht mglich, die Matrix
R
T
H
in
Matrizenform darzustellen. Deshalb wird jedes Matrizenelement einzeln gezeigt.
5
A
0
=
R
T
H
=
3
A
0
*
5
A
3
=
Gleichung 4.9
Fr die weitere Berechnung werden folgende Abkrzungen eingefhrt:
s0 = sin(q
0
); c0 = cos(q
0
);
s1 = sin(q
1
); c1 = cos(q
1
);
s2 = sin(q
2
); c2 = cos(q
2
);
s3 = sin(q
3
); c3 = cos(q
3
);
s4 = sin(q
4
); c4 = cos(q
4
);
s1p2 = sin(((Q1+Q2)*PI)/180); c1p2 = cos(((Q1+Q2)*PI)/180);
Die Elemente der Endmatrix
R
T
H
(
5
A
0
) lauten:
c11:= (s1p2*s3*c0*c4) + (c0*c1p2*c3*c4) - (s0*s4);
c12:= - (s1p2*s3*s4*c0) - (s4*c0*c1p2*c3) - (s0*c4);
c13:= + (s1p2*c0*c3) - (s3*c0*c1p2);
c14:= + (s1p2*c0*c3*b1) - (s3*c0*c1p2*b1) + (s1p2*c0*l3) + (s1*c0*l2);
c21:= (s0*s1p2*s3*c4) + (s0*c1p2*c3*c4) + (s4*c0);
c22:= - (s0*s1p2*s3*s4) - (s0*s4*c1p2*c3) + (c0*c4);
c23:= (s0*s1p2*c3) - (s0*s3*c1p2);
c24:= (s0*s1p2*c3*b1) -(s0*s3*c1p2*b1) + (s0*s1p2*l3) + (s0*s1*l2);
c31:= (s3*c1p2*c4) - (s1p2*c3*c4);
c32:= - (s3*s4*c1p2) + (s1p2*s4*c3);
c33:= (c1p2*c3) + (s1p2*s3);
c34:= (c1p2*c3*b1) + (s1p2*s3*b1) + (c1p2*l3) + (c1*l2) +(a1);
c41:= 0;
c42:= 0;
c43:= 0;
c44:= 1;
(
(
(
(
44 43 42 41
34 33 32 31
24 23 22 21
14 13 12 11
C C C C
C C C C
C C C C
C C C C
eines Roboterarms
4.4 Probleme und Test
a) Probleme bei der Berechnung von Matrizen:
- Die Reihenfolge der Rotationen und Verschiebungen mu beachtet
werden, da die Matrizenmultiplikation nicht kommutativ ist.
- Bei der Multiplikation von Matrizen nach dem Falk-Schema ( siehe Seite 4.2 )
mu der erste Faktor zwingend oben stehen.
b) Test der Koordinatentransformation vom Greiferkoordinatensystem
ins Weltkoordinatensystem:
Zur berprfung der Matrix
R
T
H
wird wie folgt vorgegangen:
- Die Matrix wird mit einem beliebigen Testvektor multipliziert
(Beispiele: P( 1,0,0,1 ) oder P( 1,1,1,1 ) ). Es soll berprft werden, ob die
Matrix den Testpunkt bei beliebigen Winkelstellungen korrekt transformiert.
- Zur Vereinfachung wurden die Winkel auf 0bzw. auf 90festgelegt.
- Bei Winkelstellung 0fr alle Gelenke entfallen die Sinusausdrcke in der
Matrix
R
T
H
, wodurch sich eine leicht berechenbare Lsung ergibt. Anhand des
Robotermodells wird berprft, ob der errechnete Wert mit dem tatschlichen
Wert aus der Modellbetrachtung bereinstimmt. Bereits die A-Matrizen werden
auf diese Weise kontrolliert. Auf Grund der Einfachheit der A-Matrizen kann
hier der Test auch fr 45Winkel erfolgen.
- Zur Kontrolle wird der Test ein zweites Mal fr 90Winkel durchgefhrt. Dabei
entfallen die Cosinusausdrcke in der Matrix. Auch hier mssen Berechnung
und Modellbetrachtung bereinstimmen.
- Anhand der Testergebnisse kann die Lsung fr beliebigen Winkel
nachvollzogen werden.
- Die berechneten berprften Matrixelemente werden in CoDeSys als
strukturierter Text programmiert. Zur erneuten Kontrolle wird der Testpunkt
eingesetzt und das Ergebnis auf den erwarteten Wert hin berprft. Die
Winkelstellungen werden manuell vorgegeben. Auf diese Weise knnen
Programmierfehler erkannt werden.
c) Aufgetretene Probleme:
- Die mathematische Funktionen COS/SIN in CoDeSys bentigen das
Bogenma. Daher werden die Winkelwerte vom Gradma ins Bogenma
umgerechnet.
eines Roboterarms
4.5 Bestimmung der absoluten Position und Orientierung
aus der Matrix
R
T
H
Aus der Matrix
R
T
H
lassen sich nun Position und Orientierung des Greifers ableiten.
a) Position:
Die Position eines Punktes lt sich bestimmen, in dem die
R
T
H
Matrix mit dem
Punktvektor P2 des Greiferkoordinatensystems multipliziert wird.
P1 =
(
(
(
(
1
. 2
. 2
. 2
Z P
Y P
X P
x
(
(
(
(
44 43 42 41
34 33 32 31
24 23 22 21
14 13 12 11
C C C C
C C C C
C C C C
C C C C
Die Gleichung lautet in strukturiertem Text:
P1.X:=(P2.X*c11) + (P2.Y*c12) +(P2.Z*c13) + (P2.V*c14);
P1.Y:=(P2.X*c21) + (P2.Y*c22) +(P2.Z*c23) + (P2.V*c24);
P1.Z:=(P2.X*c31) + (P2.Y*c32) +(P2.Z*c33) + (P2.V*c34);
P1.V:=1.0;
Der Ursprung des Greiferkoordinatensystems entspricht dem TCP. Daher ergeben
sich beim Einsetzen des Punktes ( 0, 0, 0, 1 ) die Koordinaten des TCPs im
Basiskoordinatensystem.
x = c14; Gleichung 4.10a
y = c24; Gleichung 4.10b
z = c34; Gleichung 4.10c
b) Orientierung:
Um die absoluten Orientierungswinkel des Greifers zu bestimmen, werden die
Elemente der Matrix
R
T
H
umbenannt:
(
(
(
(
44 43 42 41
34 33 32 31
24 23 22 21
14 13 12 11
C C C C
C C C C
C C C C
C C C C
=
R
T
H
=
(
(
(
(
1 0 0 0
pz az oz nz
py ay oy ny
px ax ox nx
eines Roboterarms
z
y
x
Die Orientierung eines Koordinatensystems lt sich als Rollen, Schwenken und
Aufrichten gegenber eines Basiskoordinatensystems definieren. Dieser Sachverhalt
wird in Abbildung 4.5 veranschaulicht.
Abbildung 4.5: Orientierung von Koordinatensystemen
Die Winkel berechnen sich aus der Matrix
R
T
H
nach den Formeln [ Quelle 3, Seite
119 ]:
Rollen = arctan
nx
ny
; Gleichung 4.11a
Aufrichten = arctan
) sin( ) cos( roll ny roll nx
nz
+
; Gleichung 4.11b
Schwenken = arctan
) sin( ) cos(
) cos( ) sin(
roll ax roll oy
roll ay roll ax
; Gleichung 4.11c
Mit den Gleichungen lt sich die Orientierung des Koordinatensystems im Greifer
innerhalb des Basiskoordinatensystems und somit die Orientierung des Greifers
selbst bestimmen.
Abbildung 4.6: Orientierung des Greifers im Basiskoordinatensystem
z = Rollen
y = Schwenken
x = Aufrichten
z
x
Basis
TCP
y
eines Roboterarms
4.6 Bestimmung der Jakobi-Matrix
Nachdem nun absolute Position und Orientierung bekannt sind, kann aus den
Formeln durch Ableitung Jakobi Matrix ermitteln werden.
Die Jakbi-Matrix stellt die Beziehung zwischen Differentialbewegungen und
Geschwindigkeiten her.
Es wird definiert [ Quelle 4,Seite 119 ]:
dt
dx
= D
tabtast
|
.
|
\
|
1
dt
dx
: Geschwindigkeit der kartesischen Koordinaten
abtast t : skalarwertiges Abtasintervall
a) Die Allgemeine Form der Jakobi-Matrix:
(
(
(
(
(
(
(
(
(
(
(
(
(
dt
dt
dt
dt
dz
dt
dy
dt
dx
z
y
x
= x
(
(
(
(
(
(
(
(
(
(
(
(
(
dt
dq
dt
dt
dq
dt
dq
dt
dq
dt
dq
dq
5
4
3
2
1
0
- Man erhlt die Elemente J11 bis J35 Jakobi Matrix, indem man die Elemente
C14, C24, C34 der
R
T
H
Matrix partiell abgeleitet. Aus C14 entstehen durch
partielle Ableitung nach jedem Winkel die Elemente J11, J12, J13, J14, J15
und J16.
C24 und C34 folgen dem gleichen Prinzip.
- Die Werte J41 bis J61 entstehen aus der partiellen Differentiation der
absoluten Orientierungswinkel Rollen, Schwenken und Aufrichten.
(
(
(
(
(
(
(
65 64 63 62 61
55 54 53 52 51
45 44 43 42 41
35 34 33 32 31
25 24 23 22 21
15 14 13 12 11
J J J J J
J J J J J
J J J J J
J J J J J
J J J J J
J J J J J
eines Roboterarms
b) Partielle Differentiation der absoluten Position:
Partielle Differentiation von C14:
dt
dqn
:= +( (- s1p2*s0*c3*b1) + (s3*s0*c1p2*b1) - (s1p2*s0*l3) - (s1*s0*l2))
dt
dq0
+
((c1p2*c0*c3*b1) + (s3*c0* s1p2*b1) + (c1p2*c0*l3) + (c1*c0*l2))
dt
dq1
+
((c1p2*c0*c3*b1) + (s3*c0* s1p2*b1) + (c1p2*c0*l3))
dt
dq2
(- (s1p2*c0*s3*b1) -
(c3*c0*c1p2*b1))
dt
dq3
+ 0
dt
dq4
;
dt
dqn
:=+ ((c0*s1p2*c3*b1) -(c0*s3*c1p2*b1) + (c0*s1p2*l3) + (c0*s1*l2))
dt
dq0
+(
(s0*c1p2*c3*b1) +(s0*s3*s1p2*b1) + (s0*c1p2*l3) + (s0*c1*l2))
dt
dq1
+((s0*c1p2*c3*b1) +(s0*s3*s1p2*b1) + (s0*c1p2*l3))
dt
dq2
+ ((-s0*s1p2*s3*b1) -
(s0*c3*c1p2*b1))
dt
dq3
+ 0
dt
dq4
;
c34:= (c1p2*c3*b1) + (s1p2*s3*b1) + (c1p2*l3) + (c1*l2) +(a1);
c34:= +0
dt
dq0
+ ((- s1p2*c3*b1) + (c1p2*s3*b1) - (s1p2*l3) - (s1*l2))
dt
dq1
+ ((-
s1p2*c3*b1) + (c1p2*s3*b1) - (s1p2*l3)
dt
dq2
+ ((- c1p2*s3*b1) + (s1p2*c3*b1)
dt
dq3
+
0
dt
dq4
;
Durch Ausklammern der differentiellen Winkel und durch Darstellung in Matrizenform
erhlt man die Matrixelemente J11 bis J35.
c) Partielle Differentiation der absoluten Orientierungswinkel:
Die partielle Differentiation der absoluten Orientierungswinkel ist so komplex, da
diese nur rechnergesttzt realisierbar ist. Die Aufgabe der Differentiation wird
deshalb von dem Programm Maple bernommen. Die Bedienung von Maple wird
im Anhang E nher erlutert. Da das Ergebnis der Differentiation mehrere Seiten
umfat, befindet sich ein Ausdruck im Anhang B.
eines Roboterarms
d) Eigenschaften der Jakobi Matrix:
[ Quelle 4, Seite 197 ]
Die Jakobi-Matrix stellt nun die Gelenkgeschwindigkeiten der Roboterachsen in
Beziehung zur momentanen kartesischen Dreh- und Verschiebungsgeschwindigkeit
des Greiferkoordinatensystems im Bezug auf das Basiskoordinatensystem.
Die Jakobi Matrix selbst ist keine konstante Matrix, sondern ist abhngig von der
Stellung ( Arbeitspunkt ) des Roboterarms. Dieser erschwert die Echtzeitbeziehung.
4.7 Die Invertierte der Jakobi-Matrix
[ Quelle 4, Seite 200 ]
Die Invertierte der Jakobi-Matrix wird immer dann verwendet, wenn ein Sollwert fr
die Differentialverschiebung vorgegeben wird und die entsprechenden differentiellen
Gelenkverschiebungen berechnet werden sollen.
Ihre Berechnung ist allerdings nicht trivial und es gibt viele verschiedene
Mglichkeiten auf die Invertierte zu kommen.
In vorliegenden Fall handelt es sich bei der Jakobi-Matrix um eine 5x6 Matrix. Diese
mu erst mit ihrer transponierenden 6x5 Matrix multipliziert werden, um sie
anschlieend invertieren zu knnen. Das Ergebnis einer Multiplikation der
Invertierten ( 5x5 ) mit der Transponierenden ergibt die gesuchte sogenannte
pseudoinverse Matrix.
Der Ansatz fr die Invertierung mit dem zugehrigen Programm befindet sich im
Anhang C. Das Programm realisiert die Invertierung der Jakobi-Matrix nach dem
Gauschen Eliminationsverfahren.
eines Roboterarms
4.8 Probleme und Zusammenfassung
- Auf Grund der Unberschaubarkeit und des Umfangs der partiellen
Ableitungen fr die Orientierungswinkel ist es im Rahmen dieser Studienarbeit
nicht mglich, die Jakobi-Matrix zu bestimmen. Die Programmierung der
Formeln liee sich nur unter grten Anstrengungen bewerkstelligen. Der
Start des Programmes wrde das System an seine Belastbarkeitsgrenze
bringen und voraussichtlich zu einer Zyklusberschreitung fhren.
- Aus den genannten Grnden ist daher eine Realisierung der gewnschten
Bahnplanung auf Basis der Jakobi-Matrix in der zur Verfgung stehenden Zeit
nicht mglich.
- Des Weiteren wird auf die weitere Ausprogrammierung und das Testen des
Invertierungsbausteins verzichtet.
- Statt dessen wird ein alternativer Lsungsansatz untersucht, der basierend auf
trigonometrischen Gleichungen unter bestimmten Annahmen zu einem
positiven Ergebnis fhren soll.
Studienarbeit Ansteuerung alternativer Ansatz
eines Roboterarms
5 Alternativer Lsungsansatz
5.1 Einleitung
Aufgrund der im vorangegangenen Kapitel erluterten Komplexitt der Gleichungen
und der damit verbundenen Problematik lt sich im Rahmen dieser Studienarbeit
eine effiziente Bahnplanung nicht realisieren. Daher mu ein alternativer
Lsungsweg gefunden werden.
5.2 Lsungsansatz
Da der Roboterarm nur ber fnf Gelenke verfgt, lassen sich alle Glieder in einer
zweidimensionalen Ebene ( siehe Abbildung 5.1 ) darstellen, die um die Z-Achse
rotiert. Diese Tatsache wird fr einen Lsungsansatz genutzt, der auf
trigonometrischen Gleichungen basiert und die absoluten Gelenkwinkelstellungen
liefern soll. Die Bezeichnungen der einzelnen Roboterelemente knnen dem Kapitel
4 zu entnommen werden.
Abbildung 5.1: Darstellung der Roboterglieder
in einer Ebene
Durch Vorgabe eines festen Greiferwinkels im Bezug auf ein zu greifendes Objekt
wird dieses System genauer bestimmt. Dadurch knnen die bentigten
Winkelstellungen leicht ermittelt werden.
x
y
z
Ursprung
TCP
Ebene
Q0
eines Roboterarms
Eine Rotation der in Abbildung 5.1 dargestellten Ebene um die Z-Achse erfolgt
alleine im Gelenk 0 durch Drehung um den Winkel Q0. Alle anderen Gelenke
bewirken keine Rotation um die Z-Achse. Drehungen in den Gelenken 1, 2 oder 3
fhren zu einer Bewegung der Armglieder innerhalb der Ebene. Eine Drehung um
Gelenk 4 verndert zwar die Orientierung des Greiferkoordinatensytems, hat aber
keinen Einflu auf die Position des TCPs. Dieser befindet sich immer in der Ebene.
Demnach kann der Verdrehwinkel des Handgelenkes beliebig gewhlt werden. Im
Ansteuerprogramm wird Q4 auf einen geeigneten Wert festgelegt.
Umgekehrt hat eine Drehung um Q0 keinen Einflu auf die Z-Koordinate des TCPs:
der Winkel Q0 lt sich direkt aus den X- und Y-Koordinaten der Sollposition nach
der Formel 5.1 bestimmen.
Gleichung 5.1
Dabei ist darauf zu achten, da der Wert von x nie Null sein darf. Dies entsprche
einer Winkelstellung von 90bzw. 90. Diese Einschrnkung hat allerdings keine
praktische Bedeutung, da der Drehbereich des Roboters zwischen 80und 80
liegt.
Q0 = arcsin(y/x)
eines Roboterarms
5.3 Berechnung der Winkel
Der folgende Abschnitt beschreibt die Berechnung der Winkelstellungen Q0 bis Q4.
Aus den zuvor getroffenen Annahmen entstehen folgende Darstellungen des
Roboters ( Abbildung 5.2a und 5.1b ):
Abbildung 5.2a: Darstellung der Roboterebene
Ursprung
TCP
x
y
z
0
0
0
P3
P1
P2
l0 + l1 = a1
l2
l3
l5
w2
l
w3
w
3
2
2

l5*sin()
l5*cos()
P0
P4
x3
y3
z3
P4
P3
0
0
a1
Seitenansicht:
eines Roboterarms
Abbildung 5.2b: Projektion der Roboterebene in die XY-Ebene
Folgende Rechenschritte fhren zu den gesuchten Winkelstellungen. Die
Operationen lassen sich anhand Abbildung 5.2a und 5.2b nachvollziehen.
a) Berechnung von P3:
Durch den festgelegten Greiferwinkel lsst sich der Punkt P3 ( Koordinaten des
Gelenks 4 ) mathematisch bestimmen. Dazu sind folgende Rechnungen notwendig:
- Berechnung von Q0 ( siehe Gleichung 5.1 )
Q0 = arcsin (y/x)
- Abstand der Projektion von P3 auf die XY-Ebene ( P3 )
zum Ursprung
) sin( * l5 - y x w + =
- Bestimmung der Koordinaten von P3
Gleichung 5.2
+
=
=
) cos( * 5
* ) 0 cos(
* ) 0 sin(
3
3
3
3
l z
w Q
w Q
z
y
x
P
z-Achse
Draufsicht :
w
x-Achse
y-Achse
y
x
P4
P0
P3
y3
x3
Q09
eines Roboterarms
___________________________________________________________________
Einschub: Berechnung des Abstands a zweier Punkte
=
1
1
1
1
z
y
x
P
;
=
2
2
2
2
z
y
x
P
a = z1) - (z2 y1) - (y2 x1) - (x2 + +
___________________________________________________________________
b) Berechnung des Abstands von P3 und P1:
Gleichung 5.3
___________________________________________________________________
Einschub: Kosinussatz
a = b + c - 2*b*c*cos()
___________________________________________________________________
c) Berechnung der Winkel , und mit Hilfe des Kosinussatzes:
Gleichung 5.4a
Gleichung 5.4b
Gleichung 5.4c
l = a1) - (z3 0) - (y3 0) - (x3 + +
= ARCOS (l2 + l - l3 / (2*l2*l))
= ARCOS (l + l3 - l2 / (2*l*l3))
= 180 - -
A
c
a
b
B
C
eines Roboterarms
d) Berechnung der Winkel 2, 2 und 3 mit Hilfe des Kosinussatzes:
- Berechnung der Lnge w2
3 3 3 2 z y x w + + =
- Berechnung der Lnge w3
2 z y x w + + =
- Anwendung des Kosinussatzes:
Gleichung 5.5a
Gleichung 5.5b
Gleichung 5.5c
e) Berechnung der Winkelstellungen:
Gleichung 5.6a
Gleichung 5.6b
Gleichung 5.6c
Gleichung 5.6d
Die Berechnung der Winkel Q0 bis Q3 hngt von der Lage des gewhlten
Bezugskoordinatensystems im jeweiligen Gelenk statt ( siehe Abbildung 5.3 ).
Abbildung 5.3: Berechnung von Q1, Q2 und Q3
aus den Hilfswinkeln
= ARCOS (a1 + l - w2 / (2*a1*l))
2 = ARCOS w2 + l - a1 / (2*w2*l))
3 = ARCOS (l5 + w2 - w3 / (2*l5*w2))
Q0 = arcsin(y/x)
Q1 = 180 - (+2)
Q2 = 180 -
Q3 = (+2+2) - 180
Q1
Q2
-Q3
+2
+2+3
eines Roboterarms
5.4 Realisierung der Winkelberechnung im
Ansteuerprogramm
Der Programmbaustein ABSOLUTE_WINKELBERECHNUNG beinhaltet im
Wesentlichen die Berechnungen aus Abschnitt 5.3 ( Berechnung der Winkel ). Des
Weiteren enthlt er zustzliche berwachungsfunktionen, wie Verriegelungen und
Korrekturwerte, die hier erlutert werden sollen. Mit Hilfe dieser Funktionen soll eine
Fehlbedienung durch den Anwender vermieden werden.
5.4.1 Anpassung des Greiferwinkels
Fr eine Bildverarbeitung ist es von Vorteil, da das letzte Armglied, bzw. der Greifer
senkrecht zur Bodenflche steht. Durch diese Festlegung ergibt sich der geschilderte
Rechenvorgang. Den Bereich, den der Roboter durch diese Festlegung anfahren
kann, wird durch einen bestimmten Aktionsradius begrenzt. Innerhalb dieses
Bereiches wird der Greiferwinkel auf einen festen Wert von 0eingestellt. Durch eine
Korrektur des Greiferwinkels von 45bzw. -45erweitert sich dieser Bereich wie in
Abbildung 5.4 dargestellt.
Draufsicht:
Abbildung 5.4: Aktionsbereich des Roboters
Ursprung
Aktionsbereich bei 0
Aktionsbereich mit
Winkelkorrektur
eines Roboterarms
Ist die Entfernung vom TCP zum Ursprung ( Abbildung 5.5 ) grer als der mit
Formel 5.7 berechnete maximale Radius r
max
( = 29,88 cm ), so wird der
Greiferwinkel automatisch auf 45korrigiert. Die Berechnung der Winkelstellungen
erfolgt mit dem korrigierten Wert.
Gleichung 5.7
Abbildung 5.5: Korrektur des Greiferwinkels
bei maximalem Radius
Bei Unterschreitung eines minimalen Radius r
min
( Abbildung 5.6 ) wird der
Greiferwinkel auf -45korrigiert und mit diesem Wert weitergerechnet. Der Wert fr
r
min
= 17 cm wurde durch Ausmessen am Roboter ermittelt.
Abbildung 5.6: Korrektur des Greiferwinkels bei
bei minimalem Radius
Anmerkung: Beim Greifen vom Boden mit den beiden Korrekturwerten 45und -45
mu die Z-Koordinate des TCPs um 1 cm erhht werden, um ein
Aufsetzen des Greifer zu verhindern. Der tiefste Punkt des Greifers liegt
dabei unterhalb des TCPs.
) 1 1 ( ) 3 2 ( r_max b a l l + =
TCP
Ursprung
0
a1
r
max
l2
l3
b1
TCP Ursprung
0
a1
l2
l3
b1
r
min
eines Roboterarms
5.4.2 berwachung der Berechnung
Um eine Fehlfunktion des Roboters zu verhindern, finden mehrere berwachungen
statt:
- Befindet sich das zugreifende Objekt nher als 10 cm ( kritischer Radius r
krit )
am Ursprung, wird keine Berechnung der Winkelstellung durchgefhrt und der
Baustein wird mit einer Fehlermeldung beendet. Hintergrund ist die Tatsache,
da der Greifer am Fu des Roboterarms anstoen knnte.
- Ist das Objekt weiter als 37 cm entfernt, hat der Roboter keinerlei Mglichkeit,
den Punkt zu erreichen. Dann findet ebenfalls keine Berechnung statt und der
Baustein wird mit einer Fehlermeldung beendet.
- Zustzlich werden die vom Baustein errechneten Winkelsollwerte auf ihre
Plausibilitt hin geprft. Kann ein Gelenk die ermittelte Winkelstellung nicht
einnehmen, da sie auerhalb des Stellbereiches liegt, wird wie bei den beiden
zuvor genannten Kontrollfunktionen verfahren.
Bei Auslsung eines Fehlers erfolgt keine Ansteuerung des Roboters. Der Anwender
wird darber mit der Meldung Objekt nicht erreichbar informiert.
5.4.3 Anpassung der Greiferdrehung
Die Greiferdrehung ist im Programm auf den festen Wert von -90eingestellt. Diese
Stellung kann spter an die Orientierung des zu greifenden Objektes angepat
werden ( Erfassung z.B. durch Bildverarbeitung ). Eine variable Greiferdrehung ist
allerdings nur fr senkrechtes Greifen sinnvoll. Falls der Roboterarm das Objekt mit
einem korrigierten Greiferwinkel von 45bzw. -45vom Boden greift, fhrt eine
Greiferdrehung ungleich 90bzw. -90zum Aufsetzen des Greifers ( siehe
Abbildung 5.7 ).
Abbildung 5.7: Aufsetzen des Greifers
TCP
45
eines Roboterarms
5.5 Einschrnkungen
Die Funktionalitt des Roboters wird im Ansteuerprogramm auf Greifen vom Boden
beschrnkt: Der Roboter soll einen Punkt auf der XY-Ebene anfahren, der durch
seine X- und Y-Koordinaten bestimmt wird. Eine Angabe der Z-Koordinate kann
dabei entfallen, da der Wert mit 0 festgelegt ist. Die Parameter der
Winkelberechnung werden auf diese Einschrnkung hin eingestellt.
Durch Anpassung der Parameter und Vorgabe einer Z-Koordinate wre durch das
behandelte Lsungsverfahren allerdings auch ein Betrieb im dreidimensionalen
Raum mglich.
Studienarbeit Ansteuerung Programmbeschreibung
eines Roboterarms
6 Programmbeschreibung
6.1 Beschreibung der ablaufrelevanten Programmteile
Das Kapitel beschreibt die einzelnen erstellten Bausteine, die im Ansteuerprogramm
verwendet werden. Die zugehrigen Listings des Programmcodes befinden sich im
Anhang E. Die Funktionen knnen auch anhand der Kommentierung im Listing
nachvollzogen werden:
6.1.1 Baustein ROBOTERANST
Der Baustein ROBOTERANST realisiert die bertragung der Sollwerte an den
Roboter. Ein Aufruf des Bausteins mit Sollwertbergabe erfolgt in den
Ansteuerschrittketten. Die einzelnen Programmschritte im Listing ( Anhang E )
werden zeilenweise erlutert:
- Zeile 1-5: Hier werden die Winkelsollwerte der Achsen, abhngig vom
jeweiligen Winkelbereich in einen Inkrementalwert umgewandelt.
Dies ist notwendig, da der Roboterarm mit Inkrementalwerten (
vergleiche Kapitel 2.3 ) angesteuert werden mu.
- Zeile 9-14: Mit den boolschen Eingang GREIFEN wird die Stellung des
Greifer gesteuert. Der Baustein kann dadurch fr Aufnehmen
und Ablegen verwendet werden.
- Zeile 20-37: Hier wird der Steuerbefehl mit den Inkrementalwerten an den
Roboter gesendet.
6.1.2 Baustein ABSOLUTE_WINKELBERECHNUNG
Die Funktionen, die im Baustein realisiert sind, wurden ausfhrlich in Kapitel 5
beschrieben. Daher wird hier nicht weiter auf den Programmcode eingegangen.
DasListing des Bausteins befindet sich im Anhang E.
eines Roboterarms
6.1.3 Programm STOERUNG
Das Programm realisiert die berwachung des Ansteuerprogramms und kann bei
Fehlern die Ansteuerung des Roboters verriegeln. Auerdem wertet der Baustein
eine Tastenbedienung durch den Bediener aus:
- Bei einer Quittierung durch den Bediener in der Visualisierung wird das Bit
QUIT gesetzt. Dadurch wird die Strung deaktiviert und die Strungsanzeige
erlischt. Gleichzeitig wird die Bedienung wieder freigegeben.
- Da eine Strungsauslsung durch den Bediener dominant gegenber einer
Quittierung sein soll, wird sie programmtechnisch nach der Quittierung
abgearbeitet: Bei Bettigung beider Taster ist der zuletzt ablaufende
Programmteil entscheidend. Dabei werden die bei der Quittierung
angenommenen Zustnde durch die Strung berschrieben.
- Die globale Variable INFO2 steuert die Kopfleiste der Visualisierung ( siehe
Kapitel 7 ) an. Die globale Variable INFO wird in der Ablaufsteuerung
gesetzt, wenn ein Punkt vom Roboter nicht angefahren werden kann und dient
zur Anzeige in der Visualisierung. Eine Strung oder eine nicht erreichbare
Position bewirken hier einen Farbwechsel der Kopfleiste.
6.1.4 Programm AUSWAHL
Das Programm realisiert die Aktivierung der Ablaufketten und fhrt gleichzeitig eine
Verriegelung zwischen Aufnehmen und Ablegen durch. Das Programm wertet
ebenfalls eine Tastenbedienung durch den Bediener aus:
- Die Aktivierung der Ablaufketten erfolgt durch Tastendruck in der
Visualisierung. Dabei werden die Variablen AUFNEHMEN bzw. ABLEGEN
als Startbedingung gesetzt.
- Eine Aktivierung der Ablaufsteuerung fr Aufnehmen per Tastendruck ist
jedoch nur mglich, wenn keine Strung vorliegt und nicht bereits Ablegen
angewhlt wurde. Zustzlich mu die Freigabe bei erfolgreicher
Roboterinitialisierung erfolgt sein.
- Die Verriegelung fr Ablegen ist analog strukturiert.
eines Roboterarms
6.1.5 Programm INIT
Zur Ansteuerung des Manipulators mu der im Roboter integrierte Controller
zunchst initialisiert werden. Dies erfolgt im Programm INIT. Die einzelnen
Programmschritte des Listings ( Anhang E ) werden hier zeilenweise erlutert:
- Zeile1: Das Programm INIT wird nur durchlaufen, falls die Taste
NOTAUS nicht bettigt wurde, d.h. eine Strung vorliegt.
- Zeile 4-16: Falls die Initialisierung erfolgreich war wird die Hilfsvariable
REF_INIT in Zeile 73 auf TRUE gesetzt. Die
Positionierkommandos fr Referenzfahrt bei Initialisierung
werden hier einmalig gesendet.
- Zeile 21-45: Hier findet die Kanalffnung zwischen Computer und Roboterarm
statt. Falls der Bediener in der Visualisierung die Taste INIT
bettigt hat, wird die Hilfsvariable bopen zum
Verbindungsaufbau gesetzt. Zum ffnen der Verbindung mu
die Variable ComHandle den Wert 16# FFFFFFFF haben. Erst
diese Startbedingung erlaubt es die bergabeparameter fr den
seriellen Port zu setzen. Die Parameter sind: Baudrate, Stoppbit,
Parittsbit, Timeoutbit, Puffergre, Pollzeit, Angabe des Ports (
Die Eigenschaften der Parameter sind im Programm selbst nher
erlutert ).
Ist die Kanalffnung erfolgreich verlaufen, wird bopen
zurckgesetzt und INIT1 zur Initialisierung des Controllers
gesetzt. Der lokale ComHandle wird einer globalen Variablen
bergeben. Falls keine Kommunikation zustande kommt, springt
das Programm ans Ende. Das Programm versucht im nchsten
Zyklus erneut einen Verbindungsaufbau.
- Zeile 50-79: Hier wird der Roboterarm initialisiert ( INIT1 = TRUE ).
Gestartet wird die Initialisierung, indem die beiden ersten Bytes
des Sendepuffers mit 16#03 und ETX belegt werden.
Anschlieend wird das Initialisierungskommando gesendet.
Ist die Initialisierung erfolgreich, so empfngt der PC im
Empfangspuffer 21,241,242, oder 242. Dann werden die Bytes
16#20 und ETX zur Aktivierung der Lageregelung des
Controllers werden gesendet. Nach erfolgreichem Abschlu des
Bausteins INITIALISIERUNG wird die Freigabe fr die
Ansteuerung erteilt.
eines Roboterarms
6.1.6 Baustein INFORMATION
Da auf eine globale Variable nicht direkt von einer Schrittkettenaktion nach IEC-Norm
zugegriffen werden kann, ist ein Baustein ntig, der diese Aufgabe realisiert. Die
Einbindung in die Schrittkette ist als Eingangsaktion mglich. Dabei handelt es sich
um ein Programm, das einmal bei Aktivierung eines Schrittes abgearbeitet wird.
Dort kann der Baustein beispielsweise in FUB aufgerufen werden. Seine Eingnge
werden entsprechend gesetzt. Der Zugriff auf die globale Variable erfolgt durch den
Baustein.
Der Baustein INFORMATION selbst beinhaltet ein Flip-Flop, das die Variable zur
Anzeige einer nicht erreichbaren Position steuert.
6.1.7 Baustein REFERENZ
Das Baustein REFERENZ hat die Aufgabe, den Roboter in eine vorgegebene
Referenzposition zu fahren ( Diese Position kann im Bausteincode verndert
werden ):
- Der Wert des ComHandles wird von der globalen Variablen
abgeholt. Die Ansteuerung kann nur erfolgen, wenn der
ComHandle als lokale Variable verwendet wird.
- Die Sollwerte der Achsen werden in Inkrementen angegeben (
vergleiche Kapitel 2.3 ). Die eingetragenen Werte knnen vom Benutzer
beliebig angepat werden ( z.B. fr eine Bildaufnahme).
- Mit dem boolschen Eingang OEFFNEN wird die Stellung des Greifer
gesteuert. Der Baustein kann dadurch mehrfach verwendet werden.
- Abschlieend wird der Steuerbefehl mit den Inkrementalwerten an den
Roboter gesendet.
6.1.8 HAUPTPORGRAMM
Das Hauptprogramm steuert den Ablauf aller Teilprogramme und beinhaltet das
vollstndige Ansteuerprogramm. Nur im Hauptprogramm aufgerufene Programme
werden ausgefhrt:
- Zur Verriegelung des restlichen Programmes wird das Programm
STOERUNG zuerst aufgerufen. Nach Auslsung einer Strung sind dann
sofort alle folgenden Programme ber globale Variablen verriegelt.
- Das Programm AUSWAHL aktiviert die folgenden Schrittketten fr
Aufnehmen bzw. Ablegen.
- Das Programm INIT bewirkt eine Initialisierung des Roboters. Es kann
ebenfalls den restlichen Programmablauf sperren.
- Die beiden Schrittketten AUFNEHMEN und ABLEGEN kommunizieren mit
dem Roboter und steuern den gewnschten Punkt an.
eines Roboterarms
6.1.9 Programm AUFNAHME
Abbildung 6.1 zeigt das Programm AUFNAHME, welches das Aufnehmen eines
Objektes realisiert. Das Programm ist in der Programmiersprache AS
( Ablaufsteuerung ) geschrieben. Folgende Ablauffunktion wird hier strukturiert:
Abbildung 6.1: Schrittkette ABLEGEN
eines Roboterarms
a) Sobald der Bediener in der Visualisierung den Button AUFNEHMEN bettigt,
startet der Aufnahmevorgang und die AS geht vom Schritt Init in den Schritt
UEBERNAHME_SOLLPOS. Der Start der Schrittkette ist allerdings nur
mglich, falls keine Strung vorliegt oder ABLEGEN angewhlt ist. Die
Verriegelung erfolgt in einem eigenen Baustein.
b) Nach dem Start der Schrittkette werden die aktuellen Sollwerte zur
Berechnung bernommen. Die Schrittkette schaltet sofort weiter.
c) Im Schritt BERECHNUNG werden die notwendigen Stellwerte errechnet.
Gegebenenfalls kommt es im Schritt zur Korrektur der Greiferparameter.
Gleichzeitig wird berprft, ob die gewnschte Position mit dem Manipulator
erreicht werden kann oder nicht. Diese Entscheidung fhrt zu einer
Alternativverzweigung.
d) Ist die gewnschte Position erreichbar, so wird durch die Transition
ERREICHBAR zum Schritt REFERENZPOSITION geschaltet. Der Roboter
fhrt daraufhin zur programmierten Referenzposition mit geffnetem Greifer.
Nach einer Wartezeit von 3 Sekunden wird die Hilfsvariable WEITER2 zum
Weiterschalten gesetzt.
Zum automatischen Weiterschalten der Schrittkette sind zwei Hilfsmerker
Weiter1 und Weiter2 ntig, da bei nur einem Merker die Schrittkette stehen
bleiben wrde.
e) Ist die gewnschte Position nicht erreichbar, so wird ein Abbruch des
Programms eingeleitet. Gleichzeitig erscheint fr drei Sekunden eine
Fehlermeldung in der Visualisierung. Die Schrittkette wird anschlieend
beendet.
f) Im Schritt Q0_ANFAHREN wir der Roboterarm ber das zu greifende Objekt
gefahren, um eine evtl. Kollision whrend der Anfahrt zum Objekt zu
vermeiden. Nach einer Wartezeit von 2 Sekunden wird in den nchsten Schritt
geschaltet.
g) In diesem Schritt wird der Greifer mit den errechneten Winkelsollwerten ber
dem Objekt positioniert. Nach einer Wartezeit von 4 Sekunden wird
weitergeschaltet.
h) Der Greifer wird geschlossen und das Objekt aufgenommen.
i) Das Objekt wird durch Drehung im Gelenk 1 angehoben. Auf diese Weise wird
verhindert, dader Roboter auf der Bodenflche aufsetzt: Bei gleichzeitiger
Drehung um alle Achsen kann eine Drehung in den Gelenken 2, 3 oder 4 zu
einer Kollision fhren bevor sich der Greifer hoch genug ber dem Boden
befindet.
eines Roboterarms
j) Nach Aufnahme des Objektes fhrt der Roboter mit geschlossenem Greifer
zurck in Referenzposition. Der Aufnahmproze ist abgeschlossen und die
Schrittkette wird zur Grundstellung geschaltet. Durch Rcksetzen der
Starttransition AUFNEHMEN wird eine neue Bedienung mglich.
6.1.10 Programm ABLAGE
Das Programm ABLAGE ist analog wie das Programm AUFNAHME aufgebaut.
Allerdings ist die Bewegungsabfolge beim Ablegen leicht verndert. So fhrt der
Roboter zunchst geschlossen die Referenzposition an und positioniert den Greifer
dann direkt an der Sollposition. Nach ffnen des Greifers fhrt der Roboter ber die
Sollposition, um eine Verschiebung des Objektes zu vermeiden. Abschlieend nimmt
der Greifer wieder Referenzposition ein.
Studienarbeit Ansteuerung Visualisierung
eines Roboterarms
7 Visualisierung
[ Quelle 15, Seite 265ff ]
Zur Veranschaulichung eines Prozesses fr den Bediener besteht die Mglichkeit
einer Visualisierung. Sie vermittelt eine schnelle bersicht ber das zu
automatisierende System und ermglicht eine Bedienung und Beobachtung.
Das Tool zur Erstellung von Visualisierungen ist in CoDeSys integriert: In einem
speziellen Bildeditor werden statische Prozessbilder erstellt, die zur Darstellung von
variablen Prozessgren dynamisierte Bildelemente enthalten. Dabei handelt es sich
um geometrische Elemente, die Offline gezeichnet werden und mit Variablen
verknpft werden. Den Zustnden dieser Variablen knnen bestimmte Aktionen
hinterlegt werden, die dann Online beispielsweise zu Farb- oder Formnderungen
fhren. Der Zugriff auf Prozessdaten erfolgt ebenfalls ber die dynamischen
Bildelemente: ber Ein- bzw. Ausgabefenster lassen sich die Werte der Variablen
schreiben und anzeigen, so da das Programm Online ber Maus und Tastatur
bedient werden kann.
7.1 Erstellen einer Visualisierung
Um eine Visualisierung zu erstellen, mu im Object Organizer von CoDeSys die
Registerkarte Visualisierung gewhlt werden. Der Befehl Projekt/Objekt
einfgen fgt ein neues Bild in den eingeblendeten Ordner ein. Nach Vergabe
eines Namens im Dialog 'Neue Visualisierung' und Besttigung mit OK ist das leere
Bild im Arbeitsfenster editierbar. Fr weitere Informationen steht das Handbuch
Handbuch V2.2.pdf" zur Verfgung.
7.2 Beschreibung der erstellten Visualisierung
Dieser Abschnitt beschreibt den Aufbau der erstellten Bedieneroberflche ( siehe
Abbildung 7.1 ).
Auf eine dynamische Darstellung des Roboterarms wird beim Zeichnen des Bildes
verzichtet, da zur Anzeige aller mglichen Roboterstellungen 255
6
Bilder ( 6 Achsen
mit jeweils 255 Inkrementen ) in der Visualisierung hinterlegt werden mssten.
Generell gestaltet sich die Darstellung dreidimensionaler Bewegung von Objekten als
uerst schwierig. Daher wird der Roboter als statisches Bild hinterlegt.
Da auf Grund des Rechenverfahrens keine Istwerte erfat werden, erfolgt auch keine
Anzeige der aktuellen Istposition. Der Bewegungsablauf wre auerdem zu schnell
fr eine kontinuierliche Anzeige von Positionsnderungen. Der Bediener wird statt
dessen in der Statusleiste ber den Betriebszustand des Roboters informiert.
Solange eine Aufnahme- bzw. ein Ablagevorgang luft, warnt eine entsprechende
Anzeige im Statusfeld vor Bewegungen des Roboters.
eines Roboterarms
Abbildung 7.1: erstellte Bedieneroberflche
a) Kopfleiste:
Die Kopfleiste informiert ber das Thema der Studienarbeit und dient zur Warnung
bei Prozefehlern. Bei einer Strung verfrbt sie sich rot bis zur Quittierung des
Alarms. Falls der Roboter ein zu greifendes Objekt nicht erreichen kann, wird die
Kopfleiste fr drei Sekunden ebenfalls in rot angezeigt
b) Statusleiste:
Die Anzeigefelder in der Statusleiste haben die Aufgabe, den Bediener ber den
aktuellen Betriebszustand des Roboters zu informieren. Die einzelnen Felder haben
dabei folgende Bedeutung:
- ONLINE: Bei Anzeige in grn war der Kommunikationsaufbau zum
Roboter erfolgreich und der Controller im Roboter wurde
initialisiert. Eine Bedienung kann erfolgen.
- STRUNG: Nach Aktivierung des Notaus ist die Anzeige bis zur
Quittierung rot.
- AUFNEHMEN: Solange ein Aufnahmevorgang luft, erscheint diese
Anzeige in grn.
- ABLEGEN: Solange ein Ablagevorgang luft, erscheint diese Anzeige
in grn.
- NICHT ERREICHBAR: Ist eine Position nicht erreichbar, ist die Anzeige fr drei
Sekunden rot.
a) Kopfleiste
b) Statusleiste
e) Eingabefeld
d) Notaus-
Button
c) Bedienleiste
eines Roboterarms
c) Bedienleiste:
ber die Bedienleiste erfolgt die Steuerung des Roboters. Die einzelnen
Befehlstasten haben folgende Bedeutung:
- INIT: Nach Bettigung versucht das Ansteuerprogramm, einen
bertragungskanals zum Roboter zu ffnen und der
Controller zu initialisieren. Nach erfolgreicher Ausfhrung
fhrt der Roboter in Referenzposition.
- AUFNEHMEN: Durch Bettigung der Taste wird nach Vorgabe einer
Sollposition der Aufnahmevorgang gestartet.
- ABLEGEN: Durch Bettigung der Taste wird nach Vorgabe einer
Sollposition der Ablagevorgang gestartet.
- QUITIERUNG: Die Quittiertaste dient zur Quittierung einer Strung.
Die Tasten werden durch Anklicken mit der Maus bedient und erscheinen bei
Bettigung grn.
d) Notaus-Button:
Nach Bettigung des Notaus wird eine Strung gesetzt und eine Bedienung des
Roboters ist nicht mehr mglich. Alle Ansteuerprogramme sind verriegelt. Der Notaus
bewirkt allerdings keinen Stop des Roboters bei einem laufenden Prozess. Diese
Funktion kann bei Bedarf durch Senden des Befehls 16#62 ( Positionierung
stoppen ) realisiert werden. Die Regelung auf dem Controller bleibt ebenfalls
aktiviert. Eine volstndige Abschaltung des Systems kann ebenfalls im
Anwenderprogramm durch de Befehl 16#60 ( Abschaltung Regelung ) erfolgen.
Durch Quittierung wird die Strung zurckgesetzt.
e) Eingabefeld:
In den einzelnen Eingabefeldern kann der Bediener die gewnschten Sollkoordinaten
eintragen. Die Eingabe erfolgt durch Anwhlen des entsprechenden Feldes mit der
Maus und Eingabe des Sollwertes ber die Tastatur. Nach Besttigung mit Return
wird der Wert bernommen. Da der Roboter nur vom Boden greift ist eine Vorgabe
der Z-Koordinate nicht mglich.
eines Roboterarms
7.3 Bedienung des Roboters
Eine Bettigung der Tasten AUFNEHMEN bzw. ABLEGEN ist nur nach Vorgabe
eines Sollwertes sinnvoll. Bei Bettigung werden die Sollkoordinaten ins
Berechnungsprogramm bernommen und der automatische Ablauf wird gestartet.
Eine erneute Bedienung ist erst nach Beendigung eines laufenden Auftrages
mglich. Whrend des Auftrages bleibt eine Bettigung der Tasten ohne Wirkung.
Aufnehmen und Ablegen sind im Ansteuerprogramm gegeneinander verriegelt: Ein
Aufnahmevorgang kann nur gestartet werden, wenn zur Zeit kein Ablagevorgang
luft oder umgekehrt.
Solange ein Auftrag aktiv ist, sind die entsprechenden Anzeigen der Statusanzeige
gesetzt. Nach der Ausfhrung erlischt die Anzeige und die Bedienung wird wieder
freigegebenen. Falls die Sollposition nicht erreicht werden kann, wird der Auftrag
sofort ohne Ansteuerung des Roboters abgebrochen. Darber informiert die
entsprechende Anzeige.
Die Ansteuerung des Manipulators erfolgt schrittweise: Nach Erhalt eines
Stellbefehls fhrt der Roboter zuerst in Referenzposition, positioniert sich ber dem
Objekt und nimmt es dann auf bzw. legt es ab. Anschlieend fhrt der Roboter
wieder in Referenzposition. Jeder einzelne Schritt erfolgt in einer im
Anwenderprogramm fest vorgegebenen Zeit. Daher kommt es zwischen zwei
Schritten speziell bei sehr kleinen Wegstrecken zu einer kurzen Stillstandszeit.
Studienarbeit Ansteuerung Schlusswort
eines Roboterarms
8 Schlusswort
8.1 Erkenntnisse
Das Thema dieser Studienarbeit ist ein typisches Anwendungsbeispiel aus dem
Bereich der Robotik. Hier stellen Roboterarme einen der Hauptvertreter. Deren
Steuerung nimmt in der Praxis eine entscheidende Rolle ein.
Aus den in den vorherigen Kapiteln genannten Grnden lies sich die ursprngliche
Aufgabenstellung leider nicht realisieren: Der Manipulator sollte ber differentielle
Winkelberechnung mit Linearisierung im Arbeitspunkt und Bahnplanung koordiniert
werden. Der alternative Lsungsansatz fhrt allerdings auch zu einem positivem
Ergebnis. Der Ansatz liefert mit den getroffenen Festlegungen ein einfaches und
schnelles Berechnungsverfahren und ermglicht eine effiziente Ansteuerung des
Roboters. Das Primrziel, die Ansteuerung des Roboters im kartesischen Raum,
wurde somit erreicht. In diesem Sinne wird diese Studienarbeit von den Autoren als
erfolgreich abgeschlossen erachtet.
Die Arbeit hat gezeigt, da die vorgegebenen Wege nicht immer ohne weiteres
begehbar sind. Es bedarf oftmals Erfindungsgeist und Fingerspitzengefhl um
gleichwertigen Ersatz zu finden. In diesem Fall ist dies gelungen. Gerade die
Vorgehensweise bei der Bearbeitung des Themas spiegelt nach Ansicht der Autoren
ingenieursmiges Handeln wieder. So mu der Ingenieur in der Praxis eine
kritische Betrachtung der Aufgabenstellung und des geforderten Lsungsansatzes
durchfhren. Er mu flexibel auf Probleme reagieren knnen und bei Bedarf
alternative Lsungen finden. Hierbei steht hufig ein sehr begrenzter Zeitrahmen zur
Verfgung.
Studienarbeit Ansteuerung Schlusswort
eines Roboterarms
8.2 Ergnzungen
Die generelle Ansteuerung des Roboters wurde im Verlauf der Studienarbeit
realisiert: Nach Vorgabe eines Positionssollwertes ist der Manipulator in der Lage,
diesen Punkt anzufahren und ein Objekt aufzunehmen bzw. abzulegen. Dies
geschieht allerdings ohne Kontrolle des korrekten Ausfhrung:
Als Erweiterung des vorhandenen Programms kann nun eine berwachung der
Greiferstellung durch Istwerterfassung durchgefhrt werden. Dazu lt sich der
bereits fr die Bahnplanung realisierte Baustein zur Ermittlung der Raumkoordinaten
aus den Gelenkwinkelstellung einbinden. Dieser kann berprfen, ob der Greifer die
vorgegebene Sollposition auch wirklich im Rahmen von Rechenungenauigkeiten
erreicht hat. Statt der zeitabhngigen Weiterschaltung der Ablaufschrittketten kann
dann das Erreichen der Sollposition als Weiterschaltbedingung verwendet werden. In
diesem Falle ist zustzlich eine Zeitberwachung als Abbruchskriterium zu
realisieren, die den Vorgang abbricht, wenn die Position nicht erreicht werden
konnte.
Als weitere Programmerweiterung lt sich ein berprfung der Greiferstellung
realisieren. Diese testet, ob der Greifer ein Objekt aufgenommen hat oder nicht:
Damit der Greifer das Objekt aufnimmt wird der Befehl Greifer_zu gesendet. Der
Greifer kann jedoch nicht vollstndig schlieen, da sich das Objekt zwischen den
beiden Fingern befindet. Dieser Sachverhalt lt sich durch eine Istwertabfrage
erfassen. Auf diese Weise kann ausgewertet werden, ob der Roboter das Objekt
aufgenommen hat.
Im Programm kann eine weitere Verriegelung integriert werden, die verhindert, da
der Roboter zweimal hintereinander den Befehl Aufnehmen ausfhrt. In diesem Fall
wrde der Manipulator ein bereits gegriffenes Objekt fallen lassen.
8.3 Ausblick
Das Thema Robotik bietet umfangreichen Stoff fr weitere Studienarbeiten, die an
diese Arbeit anknpfen. So wird an anderer Stelle bereits ein
Bildverarbeitungssystem untersucht, das nach optischer Bilderfassung und
Verarbeitung die Positionssollwerte an die Robotersteuerung liefert. Die
Bilderfassungskamera soll auf dem Robotergreifer installiert werden. Eine
Ansteuerung des Manipulators kann dann nach automatischer Erkennung eines zu
greifenden Objektes erfolgen.
Als weiteres Projekt knnte der Manipulator mit einem mobilen Roboter kombiniert
werden. Dieser wre in der Lage, Objekte aufzunehmen und von einem Ort zu einem
anderen zu transportieren. Dies wre Grundlage fr einen einfachen
Haushaltroboter.
Ein Installation des Roboters in einem Hochregallager ist ebenfalls denkbar. Hier
knnte der Manipulator die Aufgabe des Ein- bzw. Auslagerns vom Frderband ins
Lager und umgekehrt bernehmen.
Studienarbeit Ansteuerung Literatur
eines Roboterarms
FH Mannheim Christian von der Heyd Literatur
9 Literatur
Quellenverzeichnis Teil A:
[1] H. Sieber; L. Huber:
Mathematische Formeln,
Klett Schulbuchverlag; 1980; ISBN 3-12-718000-4
[2] R. Dillmann; M. Huck:
Informationsverarbeitung in der Robotik
Springer Verlag; 1991; ISBN 3-540-53036-3
[3] M. P. Groover; M. Weiss; R. N. Nagel; N. G. Odrey:
Robotik umfassend
McGraw-Hill Book Company; 1987; ISBN 3-89028-087-0
[4] W. E. Snyder:
Computergesteuerte Industrieroboter
VCH; 1990; ISBN 3-527-26630-5
[5] T. Westermann:
Mathematik fr Ingenieure
Springer Verlag; 1996; ISBN 3-540-61249-1
[6] L. Papula:
Mathematik fr Ingenieure; Band 2; 9. Auflage
Vieweg Verlag 2000; ISBN 3-528-84237-7
[7] D. McCloy; D. M. Harris:
Robotertechnik
VCH; 1989; ISBN 3-527-26917-7
[8] W. Weber:
Industrieroboter
Fachbuchverlag Leipzig; 2002; ISBN 3-446-21604-9
[9] R. P. Paul:
Robot manipulators
Massachuetts Institute of Technologie;
1981; ISBN 0-262-16082-X
Studienarbeit Ansteuerung Literatur
eines Roboterarms
FH Mannheim Christian von der Heyd Literatur
Quellenverzeichnis Teil B:
[10] M. Seitz:
Zusammenstellung von Unterlagen
[11] C. Hyllus:
Diplomarbeit; 2002
[12] Eurobtec GmbH:
Low-Level Protokoll TR 5 ( ROB 3 ) und IR 50 p ( ROB 3i )
[13] Eurobtec GmbH:
TR 5 ROBOT MANUAL; V2.2e;1998
[14] Eurobtec GmbH:
TBPSfr WINDOWS 95; V1.0d;1998
[15] Handbuch CoDeSys:
Handbuch V2.2.pdf"
Quellen im Internet:
[16] http://grundmueller.bei.t-online.de/robi/roboter.html
[17] http://www.contentmanagement.de/MT/OP-Roboter01/op-roboter01.html
[18] http://www.haw-hamburg.de/m/mobile-roboter/MR0102/gruppe2/bilder.html
[19] http://www.heise.de/tp/deutsch/special/robo/8965/1.html
[20] http://www.roomba.de
Studienarbeit Ansteuerung Anhang A
eines Roboterarms
FH Mannheim Christian von der Heyd Anhang A

Anhang A

SPSProgrammierung mit CoDeSys

Was ist CoDeSys

Die Bezeichnung CoDeSys steht fr Controller Development System. Es handelt
sich hierbei um eine Entwicklungsumgebung zur Erstellung von Ablaufprogrammen
fr speicherprogrammierbaren Steuerungen, die einen schnellen und
unkomplizierten Einstieg in die allgemeingltigen Sprachmittel der IEC-Norm
ermglicht. Durch Einsatz der IEC Sprachen lt sich dabei eine gute Lesbarkeit des
Quelltextes erreichen. Desweiteren wird ein modularer Programmaufbau mglich.
Die Struktur der Entwicklungsoberflche basiert auf Windows Techniken.

Struktur eines Projektes

Die Entwicklung eines Steuerungsprogrammes erfolgt in Form eines Projektes, das
alle projektierten und programmierten Daten beinhaltet. Die Entwicklungsumgebung
CoDeSys stellt dazu alle Werkzeuge zur Erstellung und Verwaltung des Projektes
zur Verfgung. Die Abspeicherung erfolgt unter einem spezifischen Projektnamen.

Der Aufbau eines Projektes ist strukturiert in Steuerungsprogramm inklusive
Bausteinen, Datentypen, Ressourcen sowie Visualisierung.

Der erste Baustein des Projektes mu immer den Namen PLC_PRG tragen. Er erfllt
die Funktion eines Hauptprogramms, von dem aus alle anderen verwendeten
Bausteine aufgerufen werden. Daher kann dieser Baustein als Kontrollorgan
betrachtet werden.

Sprachen in Codesys

Die Entwicklungsumgebung CoDeSys untersttzt die Sprachen der IEC-61131
Norm. Diese werden in grafische beziehungsweise textuelle Sprachen gegliedert.

Grafische Sprachen:
- Ablaufsprache (AS)
- Funktionsplan (FUP)
- Kontaktplan (KOP)
eines Roboterarms

Textuelle Sprachen:
- Anweisungsliste (AWL)
- Strukturierter Text (ST)

In dieser Arbeit wird nur auf die Handhabung der verwendeten Sprachen
Funktionsplan, strukturierter Text und Ablaufsprache eingegangen:

a) Funktionsplan:

Der Funktionsplan ist eine grafische Programmiersprache, die aus Netzwerken
aufgebaut ist. Jedes Netzwerk enthlt logische oder arithmetische Ausdrcke. Die
Elemente des Funktionsplans lehnen dabei an Bausteine der Digitaltechnik an.

Beispiel:

b) Strukturierter Text:

Der Hochsprache Strukturierter Text ST enthlt eine Reihe von Anweisungen und
Strukturen wie IF-Bedingungen oder While-Schleifen, die an C angelehnt sind. ST
ermglicht ein strukturiertes Programmieren, wodurch die Fehlerwahrscheinlichkeit
im Programmcode gegenber anderen Sprachen reduziert wird. Auerdem kann auf
eine Vielzahl komplexer arithmetischer Operationen zurckgegriffen werden. Die
Struktur der ST-Anweisungen lt die Programmierung umfangreicher und
komplexer Formeln in wenigen Programmzeilen zu. Verschachtelte Bedingungen
und Schleifen lassen sich ber die entsprechenden Befehle einfach realisieren.

Beispiel:

eines Roboterarms

c) Ablaufsprache:

Die Ablaufsprache ist eine grafische Sprache, die zeitliche Ablufe verschiedener
Aktionen innerhalb eines Programms realisiert. Dazu mu der Proze in sequentielle
Schritte unterteilt werden. Die Programmierung erfolgt durch spezielle
Schrittelemente, die miteinander verkettet werden. Jedem Schritt werden feste
Aktionen zuordnet. Ist ein Schritt aktiv, wird die zugehrige Aktion abgearbeitet. Die
Abfolge dieser Aktionen wird ber Weiterschaltbedingungen, den sogenannten
Transitionen, gesteuert. Auf diese Weise ist eine klare Abtrennung verschiedener
Prozezustnde mglich.

Beispiel:

Studienarbeit Ansteuerung Anhang B
eines Roboterarms
FH Mannheim Christian von der Heyd Anhang B
Anhang B
Partielle Ableitung der absoluten Orientierung
a) Element J41:
J41:= (1-
(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos
(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))^2*(-sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)-
sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q4)*cos(q0)))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)
*cos(q4)+sin(q4)*cos(q0))^2/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2
)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2);
b) Element J42:
J42:= ((sin(q0)*cos(q1+q2)*sin(q3)*sin(q4)-
sin(q0)*sin(q1+q2)*cos(q3)*sin(q4))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*co
s(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))-
(sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)+sin(q0)*cos(q1+q2)*cos(q3)*sin(q4)+sin(q4)*cos(
q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))^2*(cos(q1+q2)*sin(q3)*cos(q0)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)+sin(q0)*
cos(q1+q2)*cos(q3)*sin(q4)+sin(q4)*cos(q0))^2/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+
cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2);
c) Element J43:
J43:= ((sin(q0)*cos(q1+q2)*sin(q3)*sin(q4)-
sin(q0)*sin(q1+q2)*cos(q3)*sin(q4))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*co
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)+sin(q0)*
eines Roboterarms
d) Element J44:
J44:= ((sin(q0)*sin(q1+q2)*cos(q3)*sin(q4)-
sin(q0)*cos(q1+q2)*sin(q3)*sin(q4))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*co
sin(q0)*sin(q4))^2*(cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
cos(q1+q2)*sin(q3)*cos(q0)*cos(q4)))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)+sin(q0)*
e) Element J45:
J45:=
((sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+cos(q4)*c
os(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))-
sin(q0)*sin(q4))^2*(-sin(q1+q2)*sin(q3)*cos(q0)*sin(q4)-
cos(q0)*cos(q1+q2)*cos(q3)*sin(q4)-
sin(q0)*cos(q4)))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)+sin(q0)*cos(q1+q2)*cos(q3)*
sin(q4)+sin(q4)*cos(q0))^2/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*
cos(q3)*cos(q4)-sin(q0)*sin(q4))^2);
f) Element J51:
J51:= (sin(q3)*cos(q1+q2)*cos(q4)-
sin(q1+q2)*cos(q3)*cos(q4))/((sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q
2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))*COS(1/((1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*
cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q
1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))^2)^(1/2)))+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*c
os(q3)*cos(q4)+sin(q4)*cos(q0))^2/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(
q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*
cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*c
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))^2*((-sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)-
sin(q4)*cos(q0))*COS(1/((1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*
1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)))-
1/2*(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))*D(COS)(1/((1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q
2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*co
s(q1+q2)*cos(q3)*cos(q4)-
eines Roboterarms
sin(q0)*sin(q4))^2)^(1/2)))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2
)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos
(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))^2)^(3/2)*(1/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)
*cos(q3)*cos(q4)-sin(q0)*sin(q4))-
2*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*c
sin(q4)*cos(q0)))+2*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)
*cos(q4)+sin(q4)*cos(q0))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2
(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)-
(q0))^2/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))^2/(1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3
)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*
cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)*(-sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)-
sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q4)*cos(q0))-
1/2*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)
*cos(q0))^2/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4
)-
os(q3)*cos(q4)-
sin(q4)*cos(q0))))/(1+(sin(q3)*cos(q1+q2)*cos(q4)-
sin(q1+q2)*cos(q3)*cos(q4))^2/((sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1
+q2)*cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))^2);
eines Roboterarms
g) Element J52:
J52:= -((-sin(q3)*sin(q1+q2)*cos(q4)-
cos(q1+q2)*cos(q3)*cos(q4))/((sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+
q2)*cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))-(sin(q3)*cos(q1+q2)*cos(q4)-
2)*cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))^2*((cos(q1+q2)*sin(q3)*cos(q0)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4))*COS(1/((1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)
+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*c
os(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)))-
sin(q0)*sin(q4))^2)^(3/2)*((sin(q0)*cos(q1+q2)*sin(q3)*cos(q4)-
sin(q0)*sin(q1+q2)*cos(q3)*cos(q4))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*co
s(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)))+2*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)
*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+
cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)*(sin(q0)*cos(q1+q2)*sin(q3)*cos(q4)-
sin(q0)*sin(q1+q2)*cos(q3)*cos(q4))-
(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*
eines Roboterarms
cos(q0))^2/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)
-
cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)*(cos(q1+q2)*sin(q3)*cos(q0)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4))-
)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(3/2)*((sin(q0)*cos(q1+q2)*sin(q3)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)))))/(1+(sin(q3)*cos(q1+q2)*cos(q4)-
eines Roboterarms
h) Element J53:
J53:= -((-sin(q3)*sin(q1+q2)*cos(q4)-
2)*cos(q3)*cos(q4)-
eines Roboterarms
)-
i) Element J54:
J54:= -((-sin(q3)*sin(q1+q2)*cos(q4)-
2)*cos(q3)*cos(q4)-
eines Roboterarms
)-
eines Roboterarms
eines Roboterarms
j) Element J55:
J55:= -((-
sin(q3)*cos(q1+q2)*sin(q4)+sin(q1+q2)*cos(q3)*sin(q4))/((sin(q1+q2)*sin(q3)*cos(q0)
*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
2)*cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))^2*((-sin(q1+q2)*sin(q3)*cos(q0)*sin(q4)-
sin(q0)*cos(q4))*COS(1/((1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*
1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)))-
(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(3/2)*((-
sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)-
sin(q0)*cos(q1+q2)*cos(q3)*sin(q4)+cos(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*co
s(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2-
sin(q0)*cos(q4)))+2*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)
*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*
cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)*(-sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)-
eines Roboterarms
sin(q0)*cos(q1+q2)*cos(q3)*sin(q4)+cos(q4)*cos(q0))-
cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)*(-sin(q1+q2)*sin(q3)*cos(q0)*sin(q4)-
cos(q0)*cos(q1+q2)*cos(q3)*sin(q4)-sin(q0)*cos(q4))-
)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(3/2)*((-sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)-
sin(q0)*cos(q1+q2)*cos(q3)*sin(q4)+cos(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*co
sin(q0)*cos(q4)))))/(1+(sin(q3)*cos(q1+q2)*cos(q4)-
eines Roboterarms
k) Element J61:
J61:= (((-
sin(q0)*sin(q1+q2)*cos(q3)+sin(q0)*sin(q3)*cos(q1+q2))*(sin(q0)*sin(q1+q2)*sin(q3)*
cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*co
s(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)-(sin(q1+q2)*cos(q0)*cos(q3)-
sin(q3)*cos(q0)*cos(q1+q2))*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)
*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(
sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q4)*cos(q0))-
1/2*(sin(q1+q2)*cos(q0)*cos(q3)-
os(q3)*cos(q4)-
sin(q4)*cos(q0)))+1/2*(sin(q0)*sin(q1+q2)*cos(q3)-
sin(q0)*sin(q3)*cos(q1+q2))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+
q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*c
os(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q4)*cos(q0))))/((-
sin(q0)*sin(q4)*cos(q1+q2)*cos(q3)+cos(q0)*cos(q4))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*
s(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)-(-
sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-sin(q4)*cos(q0)*cos(q1+q2)*cos(q3)-
sin(q0)*cos(q4))*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*co
s(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos
(q3)*cos(q4)-
eines Roboterarms
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))-((sin(q1+q2)*cos(q0)*cos(q3)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)-(sin(q0)*sin(q1+q2)*cos(q3)-
os(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))/((-
(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))^2*(-1/2*(-
sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q4)*cos(q0)))-
(sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)+sin(q0)*sin(q4)*cos(q1+q2)*cos(q3)-
cos(q0)*cos(q4))*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*co
(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)+(-sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-
sin(q4)*cos(q0)*cos(q1+q2)*cos(q3)-
(q3)*cos(q4)-
sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q4)*cos(q0))+1/2*(-
eines Roboterarms
(q3)*cos(q4)-
os(q3)*cos(q4)-
sin(q4)*cos(q0)))))/(1+((sin(q1+q2)*cos(q0)*cos(q3)-
os(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))^2/((-
(q3)*cos(q4)-
eines Roboterarms
l) Element J62:
J62:=
(((cos(q1+q2)*cos(q0)*cos(q3)+sin(q3)*cos(q0)*sin(q1+q2))*(sin(q0)*sin(q1+q2)*sin(q
3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)
*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)+(sin(q1+q2)*cos(q0)*cos(q3)-
sin(q3)*cos(q0)*cos(q1+q2))*(sin(q0)*cos(q1+q2)*sin(q3)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4))-1/2*(sin(q1+q2)*cos(q0)*cos(q3)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)))-
(sin(q0)*cos(q1+q2)*cos(q3)+sin(q0)*sin(q3)*sin(q1+q2))/(1+(sin(q0)*sin(q1+q2)*sin(
q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q
3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))^2)^(1/2)+1/2*(sin(q0)*sin(q1+q2)*cos(q3)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4))))/((-sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)-
eines Roboterarms
(q3)*cos(q4)-
(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))^2*((-
sin(q0)*cos(q1+q2)*sin(q3)*sin(q4)+sin(q0)*sin(q4)*sin(q1+q2)*cos(q3))/(1+(sin(q0)*s
in(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin
(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q4))^2)^(1/2)-1/2*(-sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)-
(-
cos(q1+q2)*sin(q3)*sin(q4)*cos(q0)+sin(q4)*cos(q0)*sin(q1+q2)*cos(q3))*(sin(q0)*sin
(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q
1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)-(-sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-
eines Roboterarms
sin(q0)*cos(q4))*(sin(q0)*cos(q1+q2)*sin(q3)*cos(q4)-
(q3)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4))+1/2*(-sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-
(q3)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)))))/(1+((sin(q1+q2)*cos(q0)*cos(q3)-
(q3)*cos(q4)-
eines Roboterarms
m)Element J63:
J63:=
(((cos(q1+q2)*cos(q0)*cos(q3)+sin(q3)*cos(q0)*sin(q1+q2))*(sin(q0)*sin(q1+q2)*sin(q
3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)
*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q3)*cos(q0)*cos(q1+q2))*(sin(q0)*cos(q1+q2)*sin(q3)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4))-1/2*(sin(q1+q2)*cos(q0)*cos(q3)-
(sin(q0)*cos(q1+q2)*cos(q3)+sin(q0)*sin(q3)*sin(q1+q2))/(1+(sin(q0)*sin(q1+q2)*sin(
q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q
3)*cos(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*sin(q1+q2)*cos(q3)*cos(q4))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*
cos(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4))))/((-sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)-
eines Roboterarms
(q3)*cos(q4)-
(q3)*cos(q4)-
sin(q0)*cos(q1+q2)*sin(q3)*sin(q4)+sin(q0)*sin(q4)*sin(q1+q2)*cos(q3))/(1+(sin(q0)*s
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)))-(-
cos(q1+q2)*sin(q3)*sin(q4)*cos(q0)+sin(q4)*cos(q0)*sin(q1+q2)*cos(q3))*(sin(q0)*sin
eines Roboterarms
sin(q0)*cos(q4))*(sin(q0)*cos(q1+q2)*sin(q3)*cos(q4)-
(q3)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4))+1/2*(-sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-
(q3)*cos(q4)-
cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)))))/(1+((sin(q1+q2)*cos(q0)*cos(q3)-
sin(q0)*cos(q4))*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*
os(q3)*cos(q4)-
eines Roboterarms
n) Element J64:
J64:= (((-sin(q3)*cos(q0)*sin(q1+q2)-
cos(q1+q2)*cos(q0)*cos(q3))*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2
sin(q3)*cos(q0)*cos(q1+q2))*(sin(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
sin(q0)*cos(q1+q2)*sin(q3)*cos(q4))/(sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)+cos(q0)*co
cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)*(cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
cos(q1+q2)*sin(q3)*cos(q0)*cos(q4))-1/2*(sin(q1+q2)*cos(q0)*cos(q3)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(3/2)*((sin(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
cos(q1+q2)*sin(q3)*cos(q0)*cos(q4)))-(-sin(q0)*sin(q3)*sin(q1+q2)-
sin(q0)*cos(q1+q2)*cos(q3))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+
sin(q0)*sin(q4))^2)^(3/2)*((sin(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
cos(q1+q2)*sin(q3)*cos(q0)*cos(q4))))/((-sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)-
eines Roboterarms
(q3)*cos(q4)-
(q3)*cos(q4)-
sin(q0)*sin(q4)*sin(q1+q2)*cos(q3)+sin(q0)*cos(q1+q2)*sin(q3)*sin(q4))/(1+(sin(q0)*s
sin(q0)*sin(q4))^2)^(3/2)*((sin(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
cos(q1+q2)*sin(q3)*cos(q0)*cos(q4)))-(-
sin(q4)*cos(q0)*sin(q1+q2)*cos(q3)+cos(q1+q2)*sin(q3)*sin(q4)*cos(q0))*(sin(q0)*sin
eines Roboterarms
sin(q0)*cos(q4))*(sin(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
(q3)*cos(q4)-
cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)*(cos(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
cos(q1+q2)*sin(q3)*cos(q0)*cos(q4))+1/2*(-sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-
(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(3/2)*((sin(q0)*sin(q1+q2)*cos(q3)*cos(q4)-
cos(q1+q2)*sin(q3)*cos(q0)*cos(q4)))))/(1+((sin(q1+q2)*cos(q0)*cos(q3)-
sin(q0)*cos(q4))*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*
os(q3)*cos(q4)-
eines Roboterarms
o) Element J65:
J65:= (((sin(q1+q2)*cos(q0)*cos(q3)-sin(q3)*cos(q0)*cos(q1+q2))*(-
sin(q0)*sin(q4)*cos(q1+q2)*cos(q3)+cos(q0)*cos(q4))/(sin(q1+q2)*sin(q3)*cos(q0)*co
s(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)*(-sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-
sin(q4)*cos(q0)*cos(q1+q2)*cos(q3)-sin(q0)*cos(q4))-
1/2*(sin(q1+q2)*cos(q0)*cos(q3)-
sin(q0)*sin(q4))^3*(-sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-
sin(q0)*cos(q4)))+1/2*(sin(q0)*sin(q1+q2)*cos(q3)-
os(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(3/2)*((-
sin(q4)*cos(q0)*cos(q1+q2)*cos(q3)-sin(q0)*cos(q4))))/((-
(q3)*cos(q4)-
eines Roboterarms
(q3)*cos(q4)-
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2))^2*((-sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)-
sin(q4)*cos(q0))/(1+(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*
os(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)-1/2*(-sin(q0)*sin(q1+q2)*sin(q3)*sin(q4)-
s(q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(3/2)*((-
sin(q4)*cos(q0)*cos(q1+q2)*cos(q3)-sin(q0)*cos(q4)))-(-
sin(q1+q2)*sin(q3)*cos(q0)*cos(q4)-
cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q0)*sin(q4))*(sin(q0)*sin(q1+q2)*sin(q3)*co
s(q4)+sin(q0)*cos(q1+q2)*cos(q3)*cos(q4)+sin(q4)*cos(q0))/(sin(q1+q2)*sin(q3)*cos(
q0)*cos(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
sin(q4)*cos(q0)*cos(q1+q2)*cos(q3)-sin(q0)*cos(q4))*(-
s(q4)+cos(q0)*cos(q1+q2)*cos(q3)*cos(q4)-
eines Roboterarms
sin(q0)*cos(q4))^2*(sin(q0)*sin(q1+q2)*sin(q3)*cos(q4)+sin(q0)*cos(q1+q2)*cos(q3)*
os(q3)*cos(q4)-
cos(q3)*cos(q4)-sin(q0)*sin(q4))^2)^(1/2)+1/2*(-sin(q1+q2)*sin(q3)*sin(q4)*cos(q0)-
(q3)*cos(q4)-
sin(q0)*cos(q4)))))/(1+((sin(q1+q2)*cos(q0)*cos(q3)-
(q3)*cos(q4)-
Studienarbeit Ansteuerung Anhang C
eines Roboterarms
FH Mannheim Christian von der Heyd Anhang C
Anhang C
Progammbeschreibung der Rckwrtstransformation
a) Variablendeklaration:
VAR
D: ARRAY [1..6, 1..5] OF REAL;
E: ARRAY [1..5, 1..6] OF REAL;
VORWAERTS: JAKOBI;
RUECKWAERTS: INVERSION;
END_VAR
VAR_INPUT
dq0_v: REAL; (* Diff. Winkel Achse 0 *)
dx_r: REAL; (* Diff. Translation in x-Richtung *)
dy_r: REAL; (* Diff. Translation in y-Richtung *)
dz_r: REAL; (* Diff. Translation in z-Richtung *)
drotx_r: REAL; (* Diff. Rotations um x-Achse *)
droty_r: REAL; (* Diff. Rotations um y-Achse *)
drotz_r: REAL; (* Diff. Rotations um z-Achse *)
END_VAR
VAR_OUTPUT
dx_v: REAL; (* Diff. Translation in x-Richtung *)
dy_v: REAL; (* Diff. Translation in y-Richtung *)
dz_v: REAL; (* Diff. Translation in z-Richtung *)
drotx_v: REAL; (* Diff. Rotations um x-Achse *)
droty_v: REAL; (* Diff. Rotations um y-Achse *)
drotz_v: REAL; (* Diff. Rotations um z-Achse *)
dq0_r: REAL; (* Diff. Winkel Achse 0 *)
END_VAR
_}___
eines Roboterarms
VORWAERTS(J11=>D[1,1] , J12=>D[1,2] , J13=>D[1,3] , J14=>D[1,4] , J15=>D[1,5]
, J21=>D[2,1] , J22=>D[2,2] , J23=>D[2,3] , J24=>D[2,4] , J25=>D[2,5] , J31=>D[3,1]
, J32=>D[3,2] , J33=>D[3,3] , J34=>D[3,4] , J35=>D[3,5] , J41=>D[4,1] , J42=>D[4,2]
, J43=>D[4,3] , J44=>D[4,4] , J45=>D[4,5] , J51=>D[5,1] , J52=>D[5,2] , J53=>D[5,3]
, J54=>D[5,4] , J55=>D[5,5] , J61=>D[6,1] , J62=>D[6,2] , J63=>D[6,3] , J64=>D[6,4]
, J65=>D[6,5] );
RUECKWAERTS(B:=D , C=>E );
b) Multiplikation der Jakobi-Matrix mit den differentiellen Winkeln:
dx_v:= (D[1,1]*dq0_v) + (D[1,2]*dq1_v) + (D[1,3]*dq2_v) + (D[1,4]*dq3_v) +
(D[1,5]*dq4_v);
dy_v:= (D[2,1]*dq0_v) + (D[2,2]*dq1_v) + (D[2,3]*dq2_v) + (D[2,4]*dq3_v) +
(D[2,5]*dq4_v);
dz_v:= (D[3,1]*dq0_v) + (D[3,2]*dq1_v) + (D[3,3]*dq2_v) + (D[3,4]*dq3_v) +
(D[3,5]*dq4_v);
drotz_v:= (D[4,1]*dq0_v) + (D[4,2]*dq1_v) + (D[4,3]*dq2_v) + (D[4,4]*dq3_v) +
(D[4,5]*dq4_v);
droty_v:= (D[5,1]*dq0_v) + (D[5,2]*dq1_v) + (D[5,3]*dq2_v) + (D[5,4]*dq3_v) +
(D[5,5]*dq4_v);
drotx_v:= (D[6,1]*dq0_v) + (D[6,2]*dq1_v) + (D[6,3]*dq2_v) + (D[6,4]*dq3_v) +
(D[6,5]*dq4_v);
c) Rckwrtstransformation;
dq0_r:= E[1,1]*dx_r + E[1,2]*dy_r + E[1,3]*dz_r + E[1,4]*drotx_r + E[1,5]*droty_r
+ E[1,6]*drotz_r;
+ E[2,6]*drotz_r;
+ E[3,6]*drotz_r;
+ E[4,6]*drotz_r;
+ E[5,6]*drotz_r
eines Roboterarms
Inversion der Jakobi Matrix
a) Variablendeklaration:
VAR
fehler: BOOL;
i: INT;
j: INT;
n: INT; (* Dimension der Matrix *)
k: INT; (* Eliminationsdurchlauf *)
temp: REAL;
A: ARRAY [1..5, 1..5] OF REAL; (* zu invertierende Matrix *)
A1: ARRAY [1..5, 1..5] OF REAL; (* inverse von A *)
END_VAR
VAR_INPUT
B: ARRAY [1..6, 1..5] OF REAL;
END_VAR
VAR_OUTPUT
C: ARRAY [1..5, 1..6] OF REAL;
END_VAR_"__;
(*
(*
b) Matrix fr Inversion erweitern:
(* Multiplikation der 6x5 Matrix mit der Transponierenden (6x5)
A = B * BT
==> Ergebnis ist eine 5x5 Matrix *)
A[1,1] := (B[1,1]*B[1,1]) + (B[2,1]*B[2,1]) + (B[3,1]*B[3,1]) + (B[4,1]*B[4,1]) +
(B[5,1]*B[5,1]) + (B[6,1]*B[6,1]);
A[1,2] := (B[1,1]*B[1,2]) + (B[2,1]*B[2,2]) + (B[3,1]*B[3,2]) + (B[4,1]*B[4,2]) +
(B[5,1]*B[5,2]) + (B[6,1]*B[6,2]);
A[1,3] := (B[1,1]*B[1,3]) + (B[2,1]*B[2,3]) + (B[3,1]*B[3,3]) + (B[4,1]*B[4,3]) +
(B[5,1]*B[5,3]) + (B[6,1]*B[6,3]);
A[1,4] := (B[1,1]*B[1,4]) + (B[2,1]*B[2,4]) + (B[3,1]*B[3,4]) + (B[4,1]*B[4,4]) +
(B[5,1]*B[5,4]) + (B[6,1]*B[6,4]);
A[1,5] := (B[1,1]*B[1,5]) + (B[2,1]*B[2,5]) + (B[3,1]*B[3,5]) + (B[4,1]*B[4,5]) +
(B[5,1]*B[5,5]) + (B[6,1]*B[6,5]);
eines Roboterarms
A[2,1] := (B[1,2]*B[1,1]) + (B[2,2]*B[2,1]) + (B[3,2]*B[3,1]) + (B[4,2]*B[4,1]) +
(B[5,2]*B[5,1]) + (B[6,2]*B[6,1]);
A[2,2] := (B[1,2]*B[1,2]) + (B[2,2]*B[2,2]) + (B[3,2]*B[3,2]) + (B[4,2]*B[4,2]) +
(B[5,2]*B[5,2]) + (B[6,2]*B[6,2]);
A[2,3] := (B[1,2]*B[1,3]) + (B[2,2]*B[2,3]) + (B[3,2]*B[3,3]) + (B[4,2]*B[4,3]) +
(B[5,2]*B[5,3]) + (B[6,2]*B[6,3]);
A[2,4] := (B[1,2]*B[1,4]) + (B[2,2]*B[2,4]) + (B[3,2]*B[3,4]) + (B[4,2]*B[4,4]) +
(B[5,2]*B[5,4]) + (B[6,2]*B[6,4]);
A[2,5] := (B[1,2]*B[1,5]) + (B[2,2]*B[2,5]) + (B[3,2]*B[3,5]) + (B[4,2]*B[4,5]) +
(B[5,2]*B[5,5]) + (B[6,2]*B[6,5]);
A[3,1] := (B[1,3]*B[1,1]) + (B[2,3]*B[2,1]) + (B[3,3]*B[3,1]) + (B[4,3]*B[4,1]) +
(B[5,3]*B[5,1]) + (B[6,3]*B[6,1]);
A[3,2] := (B[1,3]*B[1,2]) + (B[2,3]*B[2,2]) + (B[3,3]*B[3,2]) + (B[4,3]*B[4,2]) +
(B[5,3]*B[5,2]) + (B[6,3]*B[6,2]);
A[3,3] := (B[1,3]*B[1,3]) + (B[2,3]*B[2,3]) + (B[3,3]*B[3,3]) + (B[4,3]*B[4,3]) +
(B[5,3]*B[5,3]) + (B[6,3]*B[6,3]);
A[3,4] := (B[1,3]*B[1,4]) + (B[2,3]*B[2,4]) + (B[3,3]*B[3,4]) + (B[4,3]*B[4,4]) +
(B[5,3]*B[5,4]) + (B[6,3]*B[6,4]);
A[3,5] := (B[1,3]*B[1,5]) + (B[2,3]*B[2,5]) + (B[3,3]*B[3,5]) + (B[4,3]*B[4,5]) +
(B[5,3]*B[5,5]) + (B[6,3]*B[6,5]);
A[4,1] := (B[1,4]*B[1,1]) + (B[2,4]*B[2,1]) + (B[3,4]*B[3,1]) + (B[4,4]*B[4,1]) +
(B[5,4]*B[5,1]) + (B[6,4]*B[6,1]);
A[4,2] := (B[1,4]*B[1,2]) + (B[2,4]*B[2,2]) + (B[3,4]*B[3,2]) + (B[4,4]*B[4,2]) +
(B[5,4]*B[5,2]) + (B[6,4]*B[6,2]);
A[4,3] := (B[1,4]*B[1,3]) + (B[2,4]*B[2,3]) + (B[3,4]*B[3,3]) + (B[4,4]*B[4,3]) +
(B[5,4]*B[5,3]) + (B[6,4]*B[6,3]);
A[4,4] := (B[1,4]*B[1,4]) + (B[2,4]*B[2,4]) + (B[3,4]*B[3,4]) + (B[4,4]*B[4,4]) +
(B[5,4]*B[5,4]) + (B[6,4]*B[6,4]);
A[4,5] := (B[1,4]*B[1,5]) + (B[2,4]*B[2,5]) + (B[3,4]*B[3,5]) + (B[4,4]*B[4,5]) +
(B[5,4]*B[5,5]) + (B[6,4]*B[6,5]);
eines Roboterarms
A[5,1] := (B[1,5]*B[1,1]) + (B[2,5]*B[2,1]) + (B[3,5]*B[3,1]) + (B[4,5]*B[4,1]) +
(B[5,5]*B[5,1]) + (B[6,5]*B[6,1]);
A[5,2] := (B[1,5]*B[1,2]) + (B[2,5]*B[2,2]) + (B[3,5]*B[3,2]) + (B[4,5]*B[4,2]) +
(B[5,5]*B[5,2]) + (B[6,5]*B[6,2]);
A[5,3] := (B[1,5]*B[1,3]) + (B[2,5]*B[2,3]) + (B[3,5]*B[3,3]) + (B[4,5]*B[4,3]) +
(B[5,5]*B[5,3]) + (B[6,5]*B[6,3]);
A[5,4] := (B[1,5]*B[1,4]) + (B[2,5]*B[2,4]) + (B[3,4]*B[3,4]) + (B[4,5]*B[4,4]) +
(B[5,5]*B[5,4]) + (B[6,5]*B[6,4]);
A[5,5] := (B[1,5]*B[1,5]) + (B[2,5]*B[2,5]) + (B[3,5]*B[3,5]) + (B[4,5]*B[4,5]) +
(B[5,5]*B[5,5]) + (B[6,5]*B[6,5]);
(*---------------------------------------------------------------------------------------------------------------
--*)
c) Invertierung der 5x5 Matrix:
Die Invertierung erfolgt nach dem Gauschen Eliminationsverfahren.
n:= 5;
- A1 mit Einheitsmatrix initialisieren
FOR i := 0 TO (n-1) DO
FOR j :=0 TO (n-1) DO
IF (i = j) THEN
A1[i+1,j+1] := 1.0;
ELSE
A1[i+1,j+1] := 0.0;
END_IF;
END_FOR;
END_FOR;
- A in Dreieckform bringen. Parallel alle Operationen auf A1 anwenden
FOR k :=0 TO (n-2) DO
IF (A[k+1,k+1] = 0) THEN (* Wenn A[k+1,k+1] == 0 dann Zeile tauschen *)
i := k+1;
WHILE ((i < n) AND (A[i+1,k+1] = 0)) DO
i := i+1;
END_WHILE;
eines Roboterarms
IF (i >= n) THEN
fehler := TRUE;
RETURN;
ELSE
fehler := FALSE;
END_IF;
FOR j := k TO (n-1) DO
temp := A[i+1,j+1]; (* Koeffizient in A vertauschen *)
A[i+1,j+1] := A[k+1,j+1];
A[k+1,j+1] := temp;
temp := A1[i+1,j+1]; (* Koeffizient in A1 vertauschen *)
A1[i+1,j+1] := A1[k+1,j+1];
A1[k+1,j+1] := temp;
END_FOR;
END_IF;
FOR i := k+1 TO (n-1) DO (* Gauss Elimination: Fuer jede Zeile ..*)
FOR j := k+1 TO (n-1) DO (* Dividiere die Zeile k durch Akk (Pivot wird
= 1) *)
A[k+1,j+1] := A[k+1,j+1] / A[k+1,k+1];
END_FOR;
FOR j := 0 TO (n-1) DO
A1[k+1,j+1] :=A1[k+1,j+1] / A[k+1,k+1];
END_FOR;
A[k+1,k+1] := 1.0;
FOR j := k+1 TO (n-1) DO (* Fuer einen Teil von A *)
A[i+1,j+1] := A[i+1,j+1] - (A[i+1,k+1]*A[k+1,j+1]);
END_FOR;
FOR j := 0 TO (n-1) DO (* Fuer alle Elemente von A1 *)
A1[i+1,j+1] := A1[i+1,j+1] - (A[i+1,k+1]*A1[k+1,j+1]);
END_FOR;
A[i+1,k+1] := 0.0;
END_FOR;
END_FOR;
eines Roboterarms
- Letzte Zeile durch Ann dividieren
A1[n,j+1] := A1[n,j+1] / A[n,n];
END_FOR;
A[n,n] := 1.0;
- Diagonalisieren
A in Diagonalform bringen. Parallel alle Operationen auf A1 anwenden
k := n-1;
WHILE ( k >= 0) DO
i := k-1;
WHILE ( i >= 0) DO (* Gauss Elimination: Fuer jede Zeile ..*)
j := n-1;
WHILE (j > k) DO
A[i+1,j+1] := A[i+1,j+1] - (A[i+1,k+1]*A[k+1,j+1]);
j := j-1;
END_WHILE;
A1[i+1,j+1] := A1[i+1,j+1] - (A[i+1,k+1]*A1[k+1,j+1]);
END_FOR;
A[i+1,k+1] := 0.0;
i := i-1;
END_WHILE;
k := k-1;
END_WHILE;
(*---------------------------------------------------------------------------------------------------------------
--------------------*)
eines Roboterarms
d) Erweiterung wieder aufheben:
transponierende Matrix (5x6) mit inverser Matrix (5x5) multiplizieren
C = BT * A1
==> Ergebnis ist eine 5x6 Matrix
C[1,1] := (A1[1,1]*B[1,1]) + (A1[1,2]*B[1,2]) + (A1[1,3]*B[1,3]) + (A1[1,4]*B[1,4]) +
(A1[1,5]*B[1,5]);
C[1,2] := (A1[1,1]*B[2,1]) + (A1[1,2]*B[2,2]) + (A1[1,3]*B[2,3]) + (A1[1,4]*B[2,4]) +
(A1[1,5]*B[2,5]);
C[1,3] := (A1[1,1]*B[3,1]) + (A1[1,2]*B[3,2]) + (A1[1,3]*B[3,3]) + (A1[1,4]*B[3,4]) +
(A1[1,5]*B[3,5]);
C[1,4] := (A1[1,1]*B[4,1]) + (A1[1,2]*B[4,2]) + (A1[1,3]*B[4,3]) + (A1[1,4]*B[4,4]) +
(A1[1,5]*B[4,5]);
C[1,5] := (A1[1,1]*B[5,1]) + (A1[1,2]*B[5,2]) + (A1[1,3]*B[5,3]) + (A1[1,4]*B[5,4]) +
(A1[1,5]*B[5,5]);
C[1,6] := (A1[1,1]*B[6,1]) + (A1[1,2]*B[6,2]) + (A1[1,3]*B[6,3]) + (A1[1,4]*B[6,4]) +
(A1[1,5]*B[6,5]);
C[2,1] := (A1[2,1]*B[1,1]) + (A1[2,2]*B[1,2]) + (A1[2,3]*B[1,3]) + (A1[2,4]*B[1,4]) +
(A1[2,5]*B[1,5]);
C[2,2] := (A1[2,1]*B[2,1]) + (A1[2,2]*B[2,2]) + (A1[2,3]*B[2,3]) + (A1[2,4]*B[2,4]) +
(A1[2,5]*B[2,5]);
C[2,3] := (A1[2,1]*B[3,1]) + (A1[2,2]*B[3,2]) + (A1[2,3]*B[3,3]) + (A1[2,4]*B[3,4]) +
(A1[2,5]*B[3,5]);
C[2,4] := (A1[2,1]*B[4,1]) + (A1[2,2]*B[4,2]) + (A1[2,3]*B[4,3]) + (A1[2,4]*B[4,4]) +
(A1[2,5]*B[4,5]);
C[2,5] := (A1[2,1]*B[5,1]) + (A1[2,2]*B[5,2]) + (A1[2,3]*B[5,3]) + (A1[2,4]*B[5,4]) +
(A1[2,5]*B[5,5]);
C[2,6] := (A1[2,1]*B[6,1]) + (A1[2,2]*B[6,2]) + (A1[2,3]*B[6,3]) + (A1[2,4]*B[6,4]) +
(A1[2,5]*B[6,5]);
eines Roboterarms
C[3,1] := (A1[3,1]*B[1,1]) + (A1[3,2]*B[1,2]) + (A1[3,3]*B[1,3]) + (A1[3,4]*B[1,4]) +
(A1[3,5]*B[1,5]);
C[3,2] := (A1[3,1]*B[2,1]) + (A1[3,2]*B[2,2]) + (A1[3,3]*B[2,3]) + (A1[3,4]*B[2,4]) +
(A1[3,5]*B[2,5]);
C[3,3] := (A1[3,1]*B[3,1]) + (A1[3,2]*B[3,2]) + (A1[3,3]*B[3,3]) + (A1[3,4]*B[3,4]) +
(A1[3,5]*B[3,5]);
C[3,4] := (A1[3,1]*B[4,1]) + (A1[3,2]*B[4,2]) + (A1[3,3]*B[4,3]) + (A1[3,4]*B[4,4]) +
(A1[3,5]*B[4,5]);
C[3,5] := (A1[3,1]*B[5,1]) + (A1[3,2]*B[5,2]) + (A1[3,3]*B[5,3]) + (A1[3,4]*B[5,4]) +
(A1[3,5]*B[5,5]);
C[3,6] := (A1[3,1]*B[6,1]) + (A1[3,2]*B[6,2]) + (A1[3,3]*B[6,3]) + (A1[3,4]*B[6,4]) +
(A1[3,5]*B[6,5]);
C[4,1] := (A1[4,1]*B[1,1]) + (A1[4,2]*B[1,2]) + (A1[4,3]*B[1,3]) + (A1[4,4]*B[1,4]) +
(A1[4,5]*B[1,5]);
C[4,2] := (A1[4,1]*B[2,1]) + (A1[4,2]*B[2,2]) + (A1[4,3]*B[2,3]) + (A1[4,4]*B[2,4]) +
(A1[4,5]*B[2,5]);
C[4,3] := (A1[4,1]*B[3,1]) + (A1[4,2]*B[3,2]) + (A1[4,3]*B[3,3]) + (A1[4,4]*B[3,4]) +
(A1[4,5]*B[3,5]);
C[4,4] := (A1[4,1]*B[4,1]) + (A1[4,2]*B[4,2]) + (A1[4,3]*B[4,3]) + (A1[4,4]*B[4,4]) +
(A1[4,5]*B[4,5]);
C[4,5] := (A1[4,1]*B[5,1]) + (A1[4,2]*B[5,2]) + (A1[4,3]*B[5,3]) + (A1[4,4]*B[5,4]) +
(A1[4,5]*B[5,5]);
C[4,6] := (A1[4,1]*B[6,1]) + (A1[4,2]*B[6,2]) + (A1[4,3]*B[6,3]) + (A1[4,4]*B[6,4]) +
(A1[4,5]*B[6,5]);
eines Roboterarms
C[5,1] := (A1[5,1]*B[1,1]) + (A1[5,2]*B[1,2]) + (A1[5,3]*B[1,3]) + (A1[5,4]*B[1,4]) +
(A1[5,5]*B[1,5]);
C[5,2] := (A1[5,1]*B[2,1]) + (A1[5,2]*B[2,2]) + (A1[5,3]*B[2,3]) + (A1[5,4]*B[2,4]) +
(A1[5,5]*B[2,5]);
C[5,3] := (A1[5,1]*B[3,1]) + (A1[5,2]*B[3,2]) + (A1[5,3]*B[3,3]) + (A1[5,4]*B[3,4]) +
(A1[5,5]*B[3,5]);
C[5,4] := (A1[5,1]*B[4,1]) + (A1[5,2]*B[4,2]) + (A1[5,3]*B[4,3]) + (A1[5,4]*B[4,4]) +
(A1[5,5]*B[4,5]);
C[5,5] := (A1[5,1]*B[5,1]) + (A1[5,2]*B[5,2]) + (A1[5,3]*B[5,3]) + (A1[5,4]*B[5,4]) +
(A1[5,5]*B[5,5]);
C[5,6] := (A1[5,1]*B[6,1]) + (A1[5,2]*B[6,2]) + (A1[5,3]*B[6,3]) + (A1[5,4]*B[6,4]) +
(A1[5,5]*B[6,5]);
*)
Studienarbeit Ansteuerung Anhang D
eines Roboterarms
FH Mannheim Christian von der Heyd Anhang D
Anhang D
Beschreibung der Software Maple
Das mathematische Anwenderprogramm Maple ist eine Software, die es dem
Anwender erlaubt, komplexe mathematische Rechenalgorithmen schnell und
anschaulich zu lsen. Da fr diese Studienarbeit ausschlielich Differentiationen
verwendet werden, wird nur auf diese eingegangen.
Vorgehensweise einer bei einer Differentiation
Beispiel: Differentiation der Funktion f(x) = arcustangens(x);
- ffnen eines neuen Projekts mit File/New;
- Eingabe des gewnschten Anwendung: Hier diff( f(x), x )
( Ableitung der Funktion f(x) nach x );
- Eingabe der gewnschten Funktion arctan(x) fr f(x);
- Return-Taste bettigen;
Das Ergebnis erscheint eine Zeile unterhalb der zu differenzierenden Funktion in der
Farbe blau ( Siehe Abbildung D ).
Abbildung D: Anwendungsbeispiel des Programms Maple
Studienarbeit Ansteuerung Anhang E
eines Roboterarms
FH Mannheim Christian von der Heyd Anhang E
Anhang E
Anhang E enthlt die Listings der erstellten Bausteine des Ansteuerprogramms:
Baustein ROBOTERANST
FUNCTION_BLOCK ROBOTERANST
VAR
dwRead: DWORD;
dwSent: DWORD;
SA6: BYTE;
SA5: BYTE;
SA4: BYTE;
SA3: BYTE;
SA1: BYTE;
SA2: BYTE;
Change_Pos: ARRAY [0..5] OF BYTE; (* nderung von Sollwert der Achsen 1-5 +Greifer *)
END_VAR
VAR_INPUT
q0:REAL;
q1:REAL;
q2:REAL;
q3:REAL;
q4:REAL;
GREIFEN:BOOL;
END_VAR
(*Berechnung der Inkrementsollwerte aus Winkelsollwerten*)
1 SA1:= REAL_TO_BYTE((q0 - 80.0)/ (- (160.0/255.0))); (* gegen Uhrzeigersinnn*)
2 SA2:=REAL_TO_BYTE((q1 - 25.0)/ (90.0/255.0)); (* im Uhrzeigersinnn*)
3 SA3:=REAL_TO_BYTE((q2 )/ (100.0/255.0)); (* im Uhrzeigersinnn*)
4 SA4:=REAL_TO_BYTE((q3 - 100.0)/(- (200.0/255.0))); (* gegen Uhrzeigersinnn*)
5 SA5:=REAL_TO_BYTE((q4 + 100.0)/ (200.0/255.0)); (* gegen Uhrzeigersinnn*)
6
7 (* Greifer auf oder zu*)
8
9 IF GREIFEN
10 THEN
11 SA6:=255;
12 ELSE
13 SA6:=0;
14 END_IF
15
16
17 (*bernahme der Positionsdaten*)
18
19 (* ==== Positionierkommandos ==== *)
20 send[0]:= 7; (*==> 16#0F: Ansteuerung aller Achsen mit RM 210000R111*)
21 (*----------------------------------------------------------------------------------*)
22 send[1]:= SA1; (*==> Sollwert Achse 1*)
23 send[2]:=SA2; (*==> Sollwert Achse 2*)
27 send[6]:= SA6; (*==> Sollwert Greifer*)
28 (*----------------------------------------------------------------------------------*)
29 send[7]:= 03; (*==> 16#03 Initialisierung des Roboters*)
30
31
32 (*Senden der Positionsdaten wenn Freigabe erteilt*)
33
34IF FREIGABE
35 THEN
36 dwSent := SysComWrite(HANDLE, ADR(send), 8, 2000); (* Senden der Sollwerte Achse 1-5 + Greifer*)
37 END_IF (* FREIGABE*)
Baustein ABSOLUTE_WINKELBERECHNUNG
FUNCTION_BLOCK ABSOLUTE_WINKELBERECHNUNG
VAR_INPUT
x: REAL;
y: REAL;
z:REAL;
END_VAR
VAR_OUTPUT
fehler:BOOL;
q0sg:REAL; (* Winkelsollwerte*)
q1sg:REAL;
q2sg:REAL;
q3sg:REAL;
q4sg:REAL;
END_VAR
VAR
Greiferwinkel:REAL;
Greiferdrehung:REAL;
q0s:REAL; (* Winkelsollwerte*)
q1s:REAL;
q2s:REAL;
q3s:REAL;
q4s:REAL;
x3:REAL;
y3:REAL;
z3:REAL;
alpha:REAL;
beta :REAL;
gamma:REAL;
alpha2:REAL;
gamma2:REAL;
gamma3 : REAL;
entfernung: REAL;
max_radius :REAL:= 29.88310559; (*Berechnung siehe Skizze*)
min_radius:REAL:=17.0; (*Wert abgemessen am Roboterarm*)
radius_kritisch :REAL :=10.0;
w:REAL ;
w2:REAL;
w3:REAL;
l: REAL;
phi: REAL;
m1:REAL;
m2:REAL;
m3:REAL;
m4:REAL;
END_VAR
Greiferwinkel:=0;
Greiferdrehung:=-90;
fehler := FALSE;
(*Berechnung Q0*)
x:= x+1; (*Korrektur-Werte *)
IF (x <> 0.0) THEN
q0s := ATAN (y/x);
(*Korrektur falls Objekt zu weit weg fr Greifwinkel 0 Grad ==> nderung auf 45 Grad*)
entfernung :=SQRT(EXPT(x-1,2)+EXPT(y,2)); (*Entfernung vom Ursprung zum Greifpunkt*)
IF (entfernung >max_radius)THEN
Greiferwinkel := 45.0;
z := z+1;
x:= x+1;
q0s := ATAN (y/x);
END_IF
IF (entfernung <min_radius) THEN
Greiferwinkel := -45.0;
q0s := ATAN (y/x);
x := x-1;
END_IF
IF (entfernung<radius_kritisch) THEN
fehler := TRUE;
RETURN;
END_IF
(*Berechnung Punkt3*)
phi := Greiferwinkel*PI/180; (*Bogenma*)
w := SQRT(EXPT(x,2)+EXPT(y,2))-SIN(phi)*l5;
x3 := COS(q0s)*w;
y3 :=SIN(q0s)*w;
z3 := z + COS(phi)*l5 ;
(*Lnge Vektor P1P2 bestimmen*)
l := SQRT(EXPT(x3,2)+EXPT(y3,2)+EXPT(z3-a1,2));
ELSE
fehler := TRUE;
RETURN;
END_IF
IF (l> (l2+l3)) THEN
Fehler := TRUE;
RETURN;
END_IF
(*Berechnung Q2*)
alpha := ACOS((EXPT(l2,2)+EXPT(l,2)-EXPT(l3,2)) / (2*l2*l));
gamma := ACOS((EXPT(l,2)+EXPT(l3,2)-EXPT(l2,2)) / (2*l*l3));
beta := PI -(alpha+gamma);
(*Berechnung der Hilfswinkel*)
w2 := SQRT(EXPT(x3,2)+EXPT(y3,2)+EXPT(z3,2));
w3 := SQRT(EXPT(x,2)+EXPT(y,2)+EXPT(z,2));
alpha2 := ACOS((EXPT(a1,2)+EXPT(l,2)-EXPT(w2,2)) / (2*a1*l));
gamma2 :=ACOS((EXPT(w2,2)+EXPT(l,2)-EXPT(a1,2)) / (2*w2*l));
gamma3 := ACOS((EXPT(l5,2)+EXPT(w2,2)-EXPT(w3,2)) / (2*l5*w2));
(*Berechnung Q1 und Q3*)
q1s := PI-(alpha+alpha2);
q2s := PI-Beta;
q3s := -(Pi-(gamma+gamma2+gamma3));
q4s := Greiferdrehung*PI /180; (*Achtung bei Greifwinkel 90 Grad ist nur Inkr. 15 oder 240 erlaubt*)
(*Anzeige der Winkel im Bogenma*)
q0sg := q0s *180/PI;
q1sg := (q1s *180/PI); (* 30 Grad Offset durch Roboterhardware vorgegeben*)
q2sg := q2s *180/PI;
q3sg := q3s *180/PI;
q4sg := q4s *180/PI;
(*berprfung ob Position erreichbar ist*)
IF ((q0sg >80)OR (q0sg<-80))
OR ((q1sg >115) OR (q1sg<25))
OR ((q2sg >100) OR (q2sg<0))
OR ((q3sg >100) OR (q3sg<-100)) THEN
Fehler := TRUE;
END_IF
Programm STOERUNG
PROGRAM STOERUNG
VAR
END_VAR
IF QUIT THEN
STOERUNG1:=FALSE;
FREIGABE:=TRUE;
END_IF
IF NOTAUS THEN
STOERUNG1:=TRUE;
FREIGABE:= FALSE;
AUFNEHMEN:=FALSE;
ABLEGEN:=FALSE;
END_IF
IF INFO OR STOERUNG1 THEN
INFO2 := TRUE;
ELSE
INFO2 := FALSE;
END_IF
Programm AUSWAHL
PROGRAM AUSWAHL
VAR
END_VAR
IF TASTE_AUF AND NOT STOERUNG1 AND NOT ABLEGEN AND FREIGABE
THEN
AUFNEHMEN := TRUE;
END_IF
IF TASTE_AB AND NOT STOERUNG1 AND NOT AUFNEHMEN AND FREIGABE
THEN
ABLEGEN:= TRUE;
END_IF
Programm INIT
PROGRAM INIT
VAR
ComHandle: DWORD := 16#FFFFFFFF; (* Freigabe fr Datenaustasch, bei 16#FFFFFFFF kein Austausch mglich *)
ComParam: COMSETTINGS;
bOpen: BOOL; (* Start Bedingung. Variable muss manuell von Bediener gesetzt werden *)
bReceive: BOOL; (* Start Bedingung. Variable muss manuell von Bediener gesetzt werden *)
dwRead: DWORD;
dwSent: DWORD;
scs_value: BOOL; (* Rckgabewert, wenn Parameter gesetzt werden konnten *)
init1: BOOL; (* Initialisierung des Roboterarms *)
REF_INIT: BOOL; (* Einmaliges Senden der Reserenzposition bei Initialisierung*)
END_VAR
VAR_INPUT
SA1: BYTE; (*Sollwerte Achse 1*)
SA2: BYTE;
SA3: BYTE;
SA4: BYTE;
SA5: BYTE;
SA6: BYTE;
END_VAR
VAR_OUTPUT
END_VAR
IF NOT NOTAUS THEN (* Not Aus Verriegelung*)
(* ==== Positionierkommandos fr Referenzfahrt bei Initialisierung ==== *)
IF REF_INIT THEN
send[0] := 7; (*==> 16#0F: Ansteuerung aller Achsen mit RM 0000R111*)
send[1]:=128; (* bernahme der der Referenzwerte*)
send[2]:= 0;
send[3]:=128;
send[4]:=150;
send[5]:= 15;
send[6]:= 0;
send[7] := 03; (*==> 16#03 Initialisierung des Roboters*)
dwSent := SysComWrite(HANDLE, ADR(send), 8, 2000); (* Senden der Sollwerte Achse 1-5 + Greifer*)
REF_INIT:= FALSE;
END_IF
(* ==== ====*)
(* ==== Kanalffnung zwischen PC <==> Roboterarm ==== *)
IF TASTE_INIT THEN
bOpen:= TRUE;
END_IF
IF bOpen THEN (* Startbedingungen*)
IF ComHandle = 16#FFFFFFFF THEN (* Falls keine Sendefreigabe vorhanden: => Setzen der bergabe Parameter...*)
(* ... fr den seriellen Port ber die Strukur Comsettings*)
ComParam.Port := COM1; (* Anabe des Ports*)
ComParam.dwBaudRate := 9600; (* Angabe der Baudrate*)
ComParam.byStopBits := 0; (* Angabe der Stoppbits 0 = ONESTOPBIT, 1=ONE5STOPBITS, 2=TWOSTOPBITS*)
ComParam.byParity := 0; (* Angabe des Parittsbit 0 = NOPARITY, 1 = ODDPARITY, 2 = EVENPARITY *)
ComParam.dwTimeOut := 0; (* Timeout der Schnittstelle in ms, Default = 0*)
ComParam.dwBuffersize := 0; (* Puffergre des internen Gertepuffers, Default = 0*)
ComParam.dwScan:= 0; (*Poll-Zeit der seriellen Schnittstelle, sollte auf 0 gesetzt sein*)
ComHandle := SysComOpen (Com1); (* aus SysOpenCom erhaltener Handle des Ports( Sendefreigabe)*)
scs_value:=SysComSetSettings(ComHandle, ADR(ComParam));
bOpen := FALSE; (* Bei Erkennung open Befehl rcksetzen*)
init1:= TRUE;
IF ComHandle = 16#FFFFFFFF THEN (*Falls keine Sendefreigabe: Sprung an Ende des Programms*)
bOpen:= TRUE; (* Bei nichtErkennung open Befehl setzen*)
RETURN;
END_IF;
END_IF;
END_IF;
(* ==== ====*)
(* ==== Initialisierung des Roboters ==== *)
IF Init1 THEN (* Initialisierung starten*)
dwSent := SysComWrite(ComHandle, ADR(send), 1, 2000); (* Senden des Initialisierungs Kommandos*)
(* (1) (2) (3) (4)*)
(* bergabe an dwSent: (1) erhaltener Handle des Ports
(2) Adresse, von der die Daten geholt werden sollen,
um auf den Port geschrieben zu werden
(3) Anzahl der Bytes, die geschrieben werden sollen
(4) Zeit in [ms], nach der die Funktion sptestens zurckkehrt*)
dwRead := SysComRead(ComHandle, ADR(receive[0]), 1, 0); (* Empfangen ob Initialisierung erfolgreich war*)
CASE receive[0] OF (* Initialisierung erfolgreich, wenn: *)
21,241,242,243: Init1:=FALSE; (* 21 = 16#15; 241 = F1;242 = F2;243 = F3;*)
FREIGABE:=TRUE;
(*==== Einschalten der Regelung ==== *)
send[0] := 97; (*==> 16#61 Motor-Regelung einschalten*)
send[1] := 03; (*==> 16#03 ETX *)
dwSent := SysComWrite(ComHandle, ADR(send), 2, 2000); (* Senden von "Regelung ein" Befehl*)
(* ==== ==== *)
(* Referenzfahrt ansteuern*)
REF_INIT:= TRUE;
(* ====- ==== *)
ELSE RETURN;
END_CASE;
END_IF
(*bergabe des Handles fr Roboteransteuerung*)
HANDLE:=ComHandle;
END_IF (* IF NOT STOPP*)
Baustein INFORMATION
FUNCTION_BLOCK INFORMATION
VAR_INPUT
SETZEN : BOOL ;
RUECKSETZEN : BOOL;
END_VAR
VAR_OUTPUT
END_VAR
VAR
FLIPFLOP: RS;
END_VAR
Baustein REFERENZ
FUNCTION_BLOCK REFERENZ
VAR
ComHandle: DWORD;
dwSent: DWORD;
END_VAR
VAR_INPUT
Oeffnen :BOOL; (*Referenzpos. mit geffnetem Greifer anfahren*)
END_VAR
ComHandle:= HANDLE;
send[0] := 7; (*==> 16#0F: Ansteuerung aller Achsen mit RM 0000R111*)
send[1]:=128; (* bernahme der der Referenzwerte*)
send[2]:= 0;
send[3]:=128;
send[4]:=150;
send[5]:= 15;
IF Oeffnen
THEN
send[6]:= 0; (*Oeffnen gesetzt ==> Greifer offen*)
ELSE
send[6]:= 255; (*Oeffnen nicht gesetzt ==> Greifer geschlossen*)
END_IF
dwSent := SysComWrite(HANDLE, ADR(send), 8, 2000); (* Senden der Sollwerte Achse 1-5 + Greifer*)
HAUPTPORGRAMM
PROGRAM PLC_PRG
VAR
END_VAR
CAL STOERUNG
CAL Auswahl
CAL INIT
CAL AUFNAHME
CAL ABLAGE
Globale Variablen
AR_GLOBAL CONSTANT
PI: REAL := 3.141592654;
END_VAR
VAR_GLOBAL
(*SOLLPOSITION*)
X_SOLL1 :REAL;
Y_SOLL1:REAL;
(*Initialisierung erfolgreich*)
FREIGABE: BOOL:=FALSE;
STOERUNG1: BOOL:=FALSE;
NOTAUS:BOOL:=FALSE;
AUFNEHMEN:BOOL:=FALSE;
TASTE_AUF:BOOL:=FALSE; (* Fr Visualisierung*)
TASTE_AB:BOOL:=FALSE; (* Fr Visualisierung*)
TASTE_INIT:BOOL:=FALSE; (* Fr Initialisierung*)
ABLEGEN:BOOL:=FALSE;
QUIT:BOOL:= FALSE;
INFO: BOOL; (*INFO-Bit "Position nicht erreichbar" Statusfeld*)
INFO2: BOOL;
(*Variablen fr Istwerterfassung*)
ISTWERT_A1: INT; (* Istwert der Achse *)
Q0: REAL; (* Winkel 0 *)
(*Variablen zur Berechnung der Winkelfunktionen*)
s0: REAL; (* sin*q0 *)
c0: REAL; (* cos*q0 *)
s1p2: REAL;
c1p2: REAL;
(*Geometrie des Roboterarms*)
l0: REAL := 8.5; (* Lnge 0 *)
l1: REAL := 19.0; (* Lnge 1 *)
l2: REAL := 20.0; (* Lnge 2 *)
l3: REAL := 13.0; (* Lnge 3 *)
l4: REAL := 0; (* Lnge 4 *)
l5: REAL := 13.0; (* Lnge 5 *)
a1: REAL:= 27.5; (* a1= l0+l1*)
b1: REAL:= 13.0; (* b1= l4+l5*)
(* Variablen zur Initialisierung und Ansteuerung*)
send: ARRAY [0..7] OF BYTE; (* Sendepuffer *)
receive: ARRAY [0..7] OF BYTE; (* Empfangspuffer *)
HANDLE: DWORD; (* bergabe des Handles fr Roboteransteuerung*)
END_VAR

Dutch Am Robot

Hochgeladen von

Dokumentinformationen

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

Dutch Am Robot

Hochgeladen von

Copyright:

Verfügbare Formate

Studienarbeit Ansteuerung

Das könnte Ihnen auch gefallen