DE102006036601A1

DE102006036601A1 - Robotersteuerung und Robotersteuerverfahren, welche eine Spline-Interpolation verwenden

Info

Publication number: DE102006036601A1
Application number: DE102006036601A
Authority: DE
Inventors: Koji Kamiya
Original assignee: Denso Wave Inc
Current assignee: Denso Wave Inc
Priority date: 2005-08-05
Filing date: 2006-08-04
Publication date: 2007-03-15
Also published as: JP2007042021A; JP4736607B2; US20070030271A1

Abstract

Eine Bestimmungseinheit bestimmt die Tangentenvektoren an dem einen und an dem anderen Ende eines Spline-Kurvensegmentes jedes Abschnittes so, dass eine Länge jedes Tangentenvektors proportional zu der einer das eine und das andere Ende des einen Spline-Kurvensegmentes verbindenden ersten geraden Linie ist und dass die Tangentenvektoren an dem einen Ende und an dem anderen Ende des einen Spline-Kurvensegmentes derart ausgerichtet sind, dass sie einen ersten Winkel und einen zweiten Winkel jeweils in einem vorbestimmten Verhältnis teilen. Der erste Winkel ist durch die erste gerade Linie und eine Verlängerung einer das eine und das andere Ende eines vorhergehenden Spline-Kurvensegmentes verbindenden zweiten geraden Linie gebildet. Der zweite Winkel ist durch die erste gerade Linie und eine Verlängerung einer das eine und das andere Ende eines nächsten Spline-Kurvensegmentes verbindenden dritten geraden Linie gebildet.

Description

QUERVERWEIS AUF VERWANDTE ANMELDUNGEN
Diese Anmeldung basiert auf der japanischen Patentanmeldung JP 2005-228089, die am 5. August 2005 eingereicht wurde. Diese Anmeldung beansprucht die Priorität der japanischen Patentanmeldung, so dass die gesamte Beschreibung davon hierin durch Inbezugnahme mit aufgenommen wird.
GEBIET DER ERFINDUNG
Die vorliegende Erfindung bezieht sich auf Robotersteuerungen und Robotersteuerverfahren, die im Stande sind, eine Spline-Interpolation auf einer Mehrzahl von in Abständen gegebenen Anweisungspunkten durchzuführen, wodurch eine Trajektorie eines Endes eines Roboterarms bestimmt wird.
HINTERGRUND DER ERFINDUNG
Playback-Roboter sind normalerweise so designed, dass eine Programmierbox (Teaching-Box) einem Robotersteuergerät eine Mehrzahl von Anweisungspunkten eines Endes eines Roboterarms (Manipulator) beibringt; diese Anweisungspunkte beinhalten einen Startpunkt, einen Endpunkt und dazwischen Zwischenpunkte. An das eine Ende des Manipulators ist eine mechanische Hand angebracht. Die Robotersteuerung ist derart programmiert, dass sie eine Interpolation auf den beigebrachten Anweisungspunkten ausführt, um eine durch die Anweisungspunkte hindurchgehende Trajektorie zu bestimmen. Als nächstes wird die Robotersteuerung derart programmiert, dass sie das eine Ende des Roboterarms entlang der bestimmten Trajektorie gleichmäßig bewegt.

Als Verfahren des Interpolierens auf einer Mehrzahl von Anweisungspunkten ist heutzutage ein Spline-Interpolationsverfahren, das eine Spline-Funktion verwendet, in allgemeiner Verwendung. Einige Beispiele des Spline-Interpolationsverfahrens sind in den ungeprüften japanischen Patentveröffentlichungen JP 564-081012 und JP H07-064620 offenbart.

Wenn der Variationsbereich eines Parameters t auf einen Bereich von 0 bis 1 festgelegt ist, ist das Spline-Interpolationsverfahren derart ausgestaltet, dass es:
den Variationsbereich in eine Mehrzahl von Abschnitte (Segmente) einteilt;
Kurven in den Abschnitten durch jeweils verschiedene Polynome interpoliert; und
die Kurven verbindet, um eine Spline-Kurve innerhalb des Bereiches von 0 bis 1 zu definieren.

Eine Kurve in jedem Abschnitt, die im folgenden als ein "Spline-Kurvensegment" bezeichnet wird, ist derart definiert, dass sie eine Mehrzahl von Bedingungen an beiden Enden davon erfüllt; wobei diese Bedingungen mit Positionen und Tangentenvektoren an beiden Enden jedes Spline-Kurvensegmentes verknüpft sind.

Insbesondere sind die benachbarten Spline-Kurvensegmente derart miteinander verknüpft, dass sie die folgenden Bedingungen (A) bis (C) erfüllen, um eine glatte Spline-Kurve (Trajektorie) zu erhalten, die durch die Anweisungspunkte in der Reihenfolge von dem Startpunkt zu dem Endpunkt verläuft:
Die Bedingung (A) ist es, dass die Positionen an den einen Enden der benachbarten Spline-Kurvensegmente miteinander zusammenfallen; wobei diese einen Enden einen Knoten zwischen den benachbarten Spline-Kurvensegmenten bilden.

Die Bedingung (B) ist es, dass die Tangentenvektoren an den einen Enden der benachbarten Spline-Kurvensegmente miteinander übereinstimmen.

Die Bedingung (C) ist es, dass die Steigungen bzw. Raten der Tangentenvektoren an den einen Enden der benachbarten Spline-Kurvensegmente miteinander übereinstimmen.

Wie oben ausgeführt, bestimmt die herkömmliche Robotersteuerung eine glatte Spline-Kurve, die nacheinander durch die Anweisungspunkte durchgeht, nach dem Einprogrammieren einer Mehrzahl von Anweisungspunkten des einen Endes des Roboterarms.

Wenn ein Abstand zwischen einem Paar von benachbarten Anweisungspunkten kürzer ist als ein anderer Abstand zwischen einem anderen Paar von benachbarten Anweisungspunkten, kann ein Spline-Kurvensegment in dem entsprechenden kurzen Intervall geschlungen sein, wie in 9 dargestellt ist. Ansonsten, wenn alle Abstände die gleiche Länge besitzen, kann es keine geschlungenen Spline-Kurvensegmente geben.

An einem Spline-Kurvensegment, das einem Teil einer zu erzielenden Trajektorie entspricht, der in einer im wesentlichen linearen Art und Weise gekrümmt sein soll, kann der Abstand der dem Spline-Kurvensegment entsprechenden benachbarten Anweisungspunkte relativ größer als ein anderer Abstand festgelegt sein, der einem anderen Teil der Trajektorie entspricht.

An einem Spline-Kurvensegment, das einem Teil einer zu erzielenden Trajektorie entspricht, der stark gekrümmt sein soll, muss der Abstand der dem Spline-Kurvensegment entsprechenden Anweisungspunkte relativ kürzer festgelegt sein, als ein anderer Abstand, der einem anderen Teil der Trajektorie entspricht.

Wie oben ausgeführt, ist es, um eine Spline-Kurve ohne geschlungene Abschnitte in einem Fall zu erreichen, bei dem eine gewünschte Trajektorie eines Endes eines Ro boterarms zumindest einen sanft gekrümmten Teil und zumindest einen stark gekrümmten Teil enthält, notwendig, einer Robotersteuerung eine Mehrzahl von Anweisungspunkten in einer solchen Art und Weise beizubringen, dass:
zumindest ein Abstand eines Paars von benachbarten Anweisungspunkten, die dem zumindest einen sanft gekrümmten Teil entsprechend kürzer ist, um identisch mit zumindest einem Abstand des anderen Paars von benachbarten Anweisungspunkten zu sein, die dem zumindest einem stark gekrümmten Teil entsprechen.

Dies bedeutet, dass es notwendig ist, der Robotersteuerung eine große Anzahl an Anweisungspunkten beizubringen, um eine Spline-Kurve ohne geschlungene Abschnitte zu erhalten. Dies kann eine lange Zeit benötigen, um den Anweisungspunktprogrammierungsvorgang durchzuführen, wodurch die Effizienz des Festlegens einer Trajektorie eines Endes eines Roboterarms verschlechtert wird.

ZUSAMMENFASSUNG DER ERFINDUNG

Angesichts des Hintergrundes ist es eine Aufgabe eines Gesichtspunktes der vorliegenden Erfindung, Robotersteuerungen und Robotersteuerverfahren bereitzustellen, die fähig sind, eine Spline-Kurve ohne geschlungene Abschnitte zu erzielen, selbst wenn eine Mehrzahl von Anweisungspunkten in der Länge voneinander verschieden sind.

Gemäß einem Aspekt der vorliegenden Erfindung ist ein Steuergerät zum Festlegen einer Spline-Kurve als eine Trajektorie eines vorbestimmten beweglichen Abschnittes eines Roboterkörpers vorgesehen. Das Steuergerät beinhaltet eine Interpoliereinheit, die derart ausgebildet ist, dass sie nacheinander Spline-Kurvensegmente interpoliert jeweils in Abschnitten, die Intervallen zwischen den Anweisungspunkten entsprechen. Das Steuergerät beinhaltet auch eine Tangentenvektor-Bestimmungseinheit, die derart ausgebildet ist, dass sie Tangentenvektoren an einem und dem anderen Ende eines Spline-Kurvensegmentes jeder der Abschnitte bestimmt, so dass eine Länge jedes der Tangentenvektoren proportional zu der einer ersten geraden Linie ist, die das eine und das andere Ende des einen Spline-Kurvensegmentes verbindet, und so dass die Tangentenvektoren an dem einen Ende und an dem anderen Ende des einen Spline-Kurvensegmentes derart gerichtet sind, dass sie jeweils mit einem vorbestimmten Verhältnis einen ersten Winkel und einen zweiten Winkel teilen. Der erste Winkel ist durch die erste gerade Linie und eine Verlängerung einer zweiten geraden Linie gebildet, die das eine und das andere Ende eines vorhergehenden Spline-Kurvensegmentes verbindet. Der zweite Winkel ist gebildet durch die erste gerade Linie und eine Verlängerung einer dritten geraden Linie, die das eine und das andere Ende eines nächsten Spline-Kurvensegmentes verbindet. Das Steuergerät beinhaltet weiter eine Verbindungseinheit, die derart ausgebildet ist, dass sie die Spline-Kurvensegmente so verbindet, dass eine Richtung eines der Tangentenvektoren an dem einen Ende des einen Spline-Kurvensegmentes im wesentlichen die gleiche ist wie die eines der Tangentenvektoren an dem gleichen Ende des vorhergehenden Spline-Kurvensegmentes, und dass eine Richtung des anderen der Tangentenvektoren an dem anderen Ende des einen Spline-Kurvensegmentes im wesentlichen die gleiche ist wie die eines der Tangentenvektoren an dem gleichen Ende des nächsten Spline-Kurvensegmentes.

Gemäß einem Gesichtspunkt der vorliegenden Erfindung ist ein Programmprodukt eingebettet in einen Speicher eines Steuergerätes vorgesehen zum Bestimmen einer Spline-Kurve als eine Trajektorie eines vorbestimmten beweglichen Abschnittes eines Roboterkörpers. Das Programmprodukt beinhaltet ein erstes Mittel zum Anweisen eines Steuergerätes, nacheinander Spline-Kurvensegmente zu interpolieren jeweils in Abschnitten, die Intervallen zwischen Anweisungspunkten entsprechen. Das Programmprodukt beinhaltet auch ein zweites Mittel zum Anweisen eines Steuergerätes, Tangentenvektoren an dem einen und an dem anderen Ende eines Spline-Kurvensegmentes jedes der Abschnitte zu bestimmen, so dass eine Länge jedes der Tangentenvektoren proportional zu der einer ersten geraden Linie ist, die das eine Ende und das andere Ende des einen Spline-Kurvensegmentes verbindet, und so dass die Tangentenvektoren an dem einen Ende und an dem anderen Ende des einen Spline-Kurvensegmentes derart gerichtet sind, dass sie mit einem vorbestimmten Verhältnis einen ersten Winkel bzw. einen zweiten Winkel teilen. Der erste Winkel ist gebildet durch die erste gerade Linie und eine Verlängerung einer zweiten geraden Linie, die das eine und das andere Ende eines vorhergehenden Spline-Kurvensegmentes verbindet. Der zweite Winkel ist gebildet durch die erste gerade Linie und eine Verlängerung einer dritten geraden Linie, die das eine und das andere Ende eines nächsten Spline-Kurvensegmentes verbindet. Das Programmprodukt beinhaltet weiter dritte Mittel zum Bewirken, dass ein Steuergerät die Spline-Kurvensegmente so verbindet, dass eine Richtung eines der Tangentenvektoren an dem einen Ende des einen Spline-Kurvensegmentes im wesentlichen die gleiche ist wie die des einen der Tangentenvektoren an dem gleichen Ende des vorhergehenden Spline-Kurvensegmentes, und dass eine Richtung des anderen der Tangentenvektoren an dem anderen Ende des einen Spline-Kurvensegmentes im wesentlichen die gleiche ist wie die des einen der Tangentenvektoren an dem anderen Ende des nächsten Spline-Kurvensegmentes.

Gemäß einem weiteren Gesichtspunkt der vorliegenden Erfindung ist ein Verfahren des Bestimmens einer Spline-Kurve als eine Trajektorie eines vorbestimmten beweglichen Abschnittes eines Roboterkörpers vorgesehen. Das Verfahren beinhaltet nacheinander Interpolieren von Spline-Kurvensegmenten jeweils in Abschnitten, die Intervallen zwischen Anweisungspunkten entsprechen. Dieses Verfahren beinhaltet auch das Bestimmen von Tangentenvektoren an einem und dem anderen Ende des einen Spline-Kurvensegmentes jedes der Abschnitte so, dass eine Länge jedes der Tangentenvektoren proportional zu der einer ersten geraden Linie ist, die ein Ende und das andere Ende des einen Spline-Kurvensegmentes verbindet, und dass die Tangentenvektoren an einem Ende und dem anderen Ende des einen Spline-Kurvensegmentes derart gerichtet sind, dass sie mit einem vorbestimmten Verhältnis einen ersten Winkel bzw. einen zweiten Winkel teilen. Der erste Winkel ist gebildet durch die erste gerade Linie und eine Verlängerung einer zweiten geraden Linie, die das eine und das andere Ende eines vorhergehenden Spline-Kurvensegmentes verbindet. Der zweite Winkel ist gebildet durch die erste gerade Linie und eine Verlängerung einer dritten geraden Linie, die das eine und das andere Ende eines nächsten Spline-Kurvensegmentes verbindet. Das Verfahren beinhaltet weiter das Verbinden der Spline-Kurvensegmente so, dass eine Richtung eines der Tangentenvektoren an dem einen Ende des einen Spline-Kurvensegmen tes im wesentlichen die gleiche ist, wie die des einen der Tangentenvektoren an dem gleichen Ende des vorhergehenden Spline-Kurvensegmentes, und dass eine Richtung des anderen der Tangentenvektoren an dem anderen Ende des einen Spline-Kurvensegmentes im wesentlichen die gleiche ist, wie die des einen der Tangentenvektoren an dem gleichen Ende des nächsten Spline-Kurvensegmentes.

KURZE BESCHREIBUNG DER ZEICHNUNGEN
Andere Aufgaben und Gesichtspunkte der Erfindung werden aus der folgenden Beschreibung der Ausführungsformen mit Bezug auf die begleitenden Zeichnungen anschaulich, in denen:
1 eine perspektivische Ansicht ist, die schematisch ein Beispiel der Hardwarekonfiguration eines Roboters gemäß einer ersten Ausführungsform der vorliegenden Erfindung darstellt;
2 ein Blockdiagramm ist, das schematisch ein Beispiel der Systemkonfiguration des in 1 dargestellten Roboters darstellt;
3 eine Ansicht ist, die schematisch Beispiele eines Geschwindigkeitsmusters und eines Bewegungswinkelmusters darstellt, die von dem in 1 und 2 dargestellten Steuergerät zu verwenden sind;
4 eine Ansicht ist, die schematisch ein Beispiel einer eine Mehrzahl von Anweisungspunkten enthaltenden Trajektorie gemäß der Ausführungsform darstellt;
5 eine Ansicht ist, die schematisch Tangentenvektoren an Enden der Spline-Kurvensegmente gemäß der Ausführungsform darstellt;
6 eine vergrößerte Ansicht ist, die schematisch den Zusammenhang zwischen einem Einheitsvektor innerhalb des ersten Terms jeder der Gleichungen (14) und (15) und dem innerhalb des zweiten Terms davon gemäß der vorliegenden Ausführungsform darstellt;
7 eine vergrößerte Ansicht ist, die schematisch den Zusammenhang zwischen einem Einheitsvektor innerhalb des ersten Terms jeder der Gleichungen (16) und (17) und dem innerhalb des zweiten Terms davon gemäß der Ausführungsform darstellt.
8 ein Ablaufdiagramm ist, das von einem in 1 dargestellten Steuergerät auszuführende Verfahrensschritte schematisch darstellt; und
9 eine Trajektorie ist, die eine Mehrzahl von Anweisungspunkten gemäß einer verwandten Technik der vorliegenden Erfindung enthält.
DETAILLIERTE BESCHREIBUNG DER AUSFÜHRUNGSFORMEN
Eine Ausführungsform der vorliegenden Erfindung wird im folgenden mit Bezug auf 1 bis 6 beschrieben werden.
Ein Roboter (Roboterarm) R gemäß der Ausführungsform beinhaltet einen Roboterkörper 1, ein Steuergerät 2 zum Steuern des Roboterkörpers 1 sowie ein Programmierhandgerät 3 als eine Teaching-Box für das Steuergerät 2. Der Roboterkörper 1 ist elektrisch verbunden mit dem Steuergerät 2 und das Steuergerät 2 ist elektrisch mit dem Programmierhandgerät 3 durch ein Kabel verbunden.
Wie in 1 dargestellt, ist der Roboterkörper 1 als ein Steuerziel für das Steuergerät 2 als zum Beispiel ein Vertikalgelenkroboter konstruiert.
Genauer besteht der Roboterkörper 1 aus einer im wesentlichen zylinderförmigen Basis 4 die auf einer horizontalen Ebene montiert ist, und einem im wesentlichen zylinderförmigen Schultergelenkstück 5, das auf der Basis 4 montiert ist, so dass die Mittenachsenrichtung des Schultergelenkstückes 5 orthogonal zu der Mittenachsenrichtung der Basis 4 ist. Das Schultergelenkstück 5 ist derart ausgebildet, dass es auf der Basis 4 um eine Mittenachse davon horizontal drehbar ist. Der Roboterkörper 1 besteht außerdem aus einem Paar von oberen Armen (oberen Armgelenkstücken) 6. Die einen Enden des Paares der oberen Arme 6 sind von den beiden Enden des Schultergelenkstückes 5 schwenkbar um die Mittenachse davon in der vertikalen Richtung gehalten, die der Mittenachsenrichtung der Basis 4 entspricht.
Der Roboterkörper 1 besteht aus einem unterem Arm (unterem Armgelenkstück) 7, der von den anderen Enden der oberen Arme 6 schwenkbar in der vertikalen Richtung gehalten wird, die der Mittenachsenrichtung der Basis 4 entspricht.
Der untere Arm 7 beinhaltet eine Basis 7a und einen Armkörper 7b, der sich von der Basis 7a erstreckt und dadurch um die Erstreckungsrichtung drehbar gehalten ist. Das spitzenseitige Ende des Armkörpers 7b ist gegabelt.
Der Roboterkörper 1 umfasst ein im wesentlichen zylinderförmiges Handgelenkstück 8, das zwischen die gegabelten Enden des Armkörpers 7b eingefügt und dadurch in der vertikalen Richtung schwenkbar gehalten ist, die der Mittenachsenrichtung der Basis 4 entspricht.
Das Handgelenkstück 8 ist mit einem Flansch 9 ausgebildet, der davon hervorsteht. Der Flansch 9 besitzt ein spitzenseitiges Ende derart, dass es um die Vorsprungsrichtung drehbar ist, so dass das spitzenseitige Ende die Befestigung einer mechanischen Hand (mechanischer Greifer) zulässt, die verschiedene Arten von Werkzeugen greifen kann. Das spitzenseitige Ende 9a des Flansches 9 wird im folgenden auch als ein "Handanschlussstück" bezeichnet.
Der Roboter R beinhaltet außerdem eine Anzahl von Motoren 10 als Aktuatoren. Zum Beispiel können bei der Ausführungsform DC-(Gleichstrom-)Servomotoren vorzugsweise als die Motoren 10 verwendet werden.
Insbesondere ist in die Basis 4 zumindest einer der Motoren 10 und ein Rotationsübertragungssystem (nicht dargestellt) integriert, das die Rotation von zumindest einem der Motoren 10 auf das Schultergelenkstück 5 derart überträgt, dass es horizontal gedreht wird.
In das Schultergelenkstück 5 ist zumindest einer der Motoren 10 und ein Rotationsübertragungssystem (nicht dargestellt) integriert, das die Rotation von zumindest einem der Motoren 10 an die oberen Arme 6 derart überträgt, dass sie zusammen miteinander vertikal geschwenkt werden.
In die oberen Armgelenkstücke 6 sind zumindest einer der Motoren 10 und ein Rotationsübertragungssystem (nicht dargestellt) integriert, das die Rotation von zumindest einem der Motoren 10 an das untere Armgelenkstück 7 derart überträgt, dass es vertikal geschwenkt wird. In die Basis 7a des unteren Armgelenkstückes 7 sind zumindest einer der Motoren 10 und ein Rotationsübertragungssystem (nicht dargestellt) integriert, das die Rotation von zumindest einem der Motoren 10 auf den Armkörper 7b des unteren Armgelenkstückes 7 derart überträgt, dass er sich um die Erstreckungsrichtung dreht.
In den Armkörper 7b ist zumindest einer der Motoren 10 und ein Rotationsübertragungssystem (nicht dargestellt) integriert, das die Rotation von zumindest einem der Motoren 10 an das Handgelenkstück 8 derart überträgt, dass es vertikal schwenkt. In den Flansch 9 ist zumindest einer der Motoren 10 und ein Rotationsübertragungssystem (nicht dargestellt) integriert, das die Rotation von zumindest einem der Motoren 10 auf dessen Handgelenk 9a (zusammen mit der mechanischen Hand) derart überträgt, dass es sich um die Vorsprungsrichtung des Flansches 9 dreht.
Wie in 2 dargestellt, beinhaltet das Steuergerät eine CPU 11, die als eine Steuereinheit dient, Ansteuerschaltungen 12 zum Ansteuern der Motoren 10, eine Erfassungsschaltung 13, einen ROM (Festwertspeicher, wie z. B. ein EEPROM, ein Flash-ROM oder dergleichen) 14, einen RAM (Direktzugriffsspeicher) 15 sowie eine Schnittstelle (UF) 16. Die Elemente 12 bis 16 sind elektrisch mit der CPU 11 verbunden.
Der ROM 14 weist darin gespeicherte Systemprogramme auf, die bewirken, dass die CPU 11 die Systemressourcen des Roboters R steuert.
Der RAM 15 weist ein darin gespeichertes Roboterbewegungsprogramm und dergleichen auf.
Die I/F 16 ist derart konstruiert, dass sie mit dem Programmierhandgerät 3 elektrisch zu verbinden ist.
Es sei bemerkt, dass in 2 das Schultergelenkstück 5, die oberen Armgelenkstücke 6, das untere Armgelenkstück 7, das Handgelenkstück 8 und das Handanschlussstück 9a des Flansches 9 gemeinsam als ein bewegliches Teil durch einen Block dargestellt sind, dem die Bezugszeichen 5, 6, 7, 8 und 9a zugeordnet sind. Die Antriebsquellen der beweglichen Teile (Gelenkstückabschnitte) 5, 6, 7, 8 und 9a sind gemeinsam als ein Block dargestellt, dem das Bezugszeichen 10 zugeordnet ist.
Die Erfassungsschaltung 13 ist derart funktionsfähig, dass sie die gegenwärtige Position und die gegenwärtige Winkelgeschwindigkeit jedes der Verbindungsstückabschnitte 5, 6, 7, 8 und 9a erfasst.
Insbesondere ist ein Drehgeber 17 an z. B. der Drehwelle jedes der Motoren 10 angebracht und ist elektrisch mit der Erfassungsschaltung 13 verbunden.
Der Drehgeber 17 dient sowohl als Positionssensor als auch als Geschwindigkeitssensor. Insbesondere ist der Drehgeber 17 derart konfiguriert, dass er digitale Impulse ausgibt, die der inkrementellen Winkelbewegung (Drehung) der Drehwelle jedes der Motoren 10 entsprechen. Das Impulssignal, das aus der Folge der digitalen Impulse besteht, wird an die Erfassungsschaltung 13 gegeben.
Die Erfassungsschaltung 13 ist derart funktionsfähig, dass sie auf der Grundlage des von jedem der Drehgeber 17 gesendeten Impulssignals die gegenwärtige Position der Drehwelle jedes der Motoren 10 und daher die gegenwärtige Position jedes der Gelenkstückabschnitte 5, 6, 7, 8 und 9a erfasst.
Zusätzlich ist die Erfassungsschaltung 13 derart funktionsfähig, dass sie:
die Anzahl der Impulse in dem von jedem der Drehgeber 17 eingegebenen Impulssignal pro Zeiteinheit zählt; und
auf der Grundlage des Zählergebnisses die gegenwärtige Winkelgeschwindigkeit der Drehwelle jedes der Motoren 10 und damit die gegenwärtige Drehgeschwindigkeit jeder der Gelenkstückabschnitte 5, 6, 7, 8 und 9a erfasst.
Die Erfassungsschaltung 13 ist außerdem derart funktionsfähig, dass sie jeder der Ansteuerschaltungen 12 und der CPU 11 Information gibt, welche die gegenwärtige Position und die gegenwärtige Winkelgeschwindigkeit jeder der Gelenkstückabschnitte 5, 6, 7, 8 und 9a anzeigt.
Jede der Ansteuerschaltungen ist derart funktionsfähig, dass sie:
einen Positionsanweisungswert und einen Winkelgeschwindigkeitsanweisungswen für die Bewegung jedes der beweglichen Teile 5, 6, 7b, 8 und 9a, die von der CPU 11 vorgegeben werden, mit der gegenwärtigen Position und der gegenwärtigen Winkelgeschwindigkeit jeder der Gelenkstückabschnitte 5, 6, 7, 8 und 9a vergleicht, die jeweils von der Erfassungsschaltung 13 übergeben werden; und
an jeden der entsprechenden Motoren 10 einen Ansteuerstrom liefert, der von der Abweichung zwischen dem Positionsanweisungswert für die Bewegung jedes der Gelenkstückabschnitte 5, 6, 7, 8 und 9a und der gegenwärtigen Position für die Bewegung davon sowie derjenigen zwischen dem Winkelgeschwindigkeitsanweisungswert für die Bewegung jedes der Gelenkstückabschnitte 5, 6, 7, 8 und 9a und der gegenwärtigen Winkelgeschwindigkeit davon abhängt, wodurch jeder der Motoren in drehbarer Art und Weise angesteuert wird.
Dies ermöglicht, dass die vorbestimmte Mittenposition des spitzenseitigen Endes 9a des Flansches 9, das dem einen Ende des Roboterkörpers 1 entspricht, sich entlang einer Trajektorie bewegt, die durch die den Positionsanweisungswerten entsprechenden Anweisungspunkte verläuft. Dies macht es möglich, dass der Roboterkörper 1 verschiedene Arbeiten ausführen kann.
In dem Roboterbewegungsprogramm, das in dem RAM 15 zu speichern ist, sind Parameter einschließlich eines Geschwindigkeitskoeffizienten und eines Beschleunigungs-/Abbremsungs-Koeffizienten jedes der Motoren 10 vorher aufgezeichnet worden.
Der Geschwindigkeitskoeffizient ist definiert als das Verhältnis der Maximalgeschwindigkeit der Bewegung des Arms zu einer maximalen erlaubten Geschwindigkeit jedes der Motoren 10. Der Beschleunigungs-/Abbremsungs-Koeffizient ist außerdem definiert als das Verhältnis der Beschleunigung/Abbremsung der Bewegung des Arms zu einer maximal erlaubten Beschleunigung/Abbremsung jedes der Motoren 10.
Die maximal erlaubte Geschwindigkeit und die maximal erlaubte Beschleunigung/Abbremsung sind bestimmt worden im Hinblick auf die Leistungsfähigkeit jedes der Motoren 10 und diejenige jedes der Rotationsübertragungssysteme zum Übertragen von Rotation von zumindest einem der Motoren 10 auf ein entsprechendes der Gelenkstücke unter der Bedingung, dass das Lastdrehmoment jedes der Motoren 10 nicht ein maximal erlaubtes Drehmoment davon übersteigen wird.
Die CPU 11 ist derart funktionsfähig, dass sie ein Geschwindigkeitsmuster der Bewegung des einen Endes des Roboterkörpers 1 von seiner Anfangsposition zu seiner Endposition auf ein trapezoides Muster anwendet, wobei die in dem Bewegungsprogramm gespeicherten Parameter verwendet werden, wodurch ein trapezoides Geschwindigkeitsmuster der Bewegung des einen Endes des Roboterkörpers 1 bestimmt wird.
Zusätzlich ist die CPU 11 derart funktionsfähig, dass sie:
eine Winkelgeschwindigkeit und eine Position (Ort) jedes Verbindungsstückes des Roboterkörpers 1 auf der Grundlage des bestimmten trapezoiden Geschwindigkeitsmusters nach jedem Ablauf einer vorbestimmten Zeitspanne berechnet; und
die berechnete Winkelgeschwindigkeit und Position an die Ansteuerschaltung 12 als den Winkelgeschwindigkeitsanweisungswert und den Positionsanweisungswert übergibt.
Insbesondere besteht das trapezoide Geschwindigkeitsmuster wie durch 3(a) dargestellt aus einem ersten Abschnitt, der einer Beschleunigungsphase während einer Zeitspanne T1 entspricht, einem zweiten Abschnitt, der einer Konstantgeschwindigkeitsphase mit einer maximalen Geschwindigkeit während einer Zeitspanne T2 entspricht und einem dritten Abschnitt, der einer Abbremsungsphase T3 während einer Zeitspanne T3 entspricht. Ein Muster eines Bewegungswinkels (Position), der dem in 3(a) dargestellten trapezoiden Geschwindigkeitsmuster entspricht, ist in 3(b) dargestellt.
In dem in 3(b) dargestellten Bewegungswinkelmuster wird der Bewegungswinkel eines Gelenkstückes zu einem Zeitpunkt "tn" durch θn wiedergegeben und der Bewegungswinkel des Gelenkstückes zu einem Zeitpunkt "t(n + 1)" nach Ablauf von Δt nach dem Zeitpunkt "tn" wird durch "θ(n + 1)" wiedergegeben.
Die Winkeländerung nach Ablauf von Δt entspricht der durch "V(n + 1)" wiedergegebenen Winkelgeschwindigkeit des Gelenkstückes zu einem Zeitpunkt "t(n + 1)". Die Winkelgeschwindigkeit V(n + 1) wird als der Winkelgeschwindigkeitsanweisungswert zu dem Zeitpunkt "tn" verwendet. Der Winkelgeschwindigkeitsanweisungswert (die Winkelgeschwindigkeit V(n + 1)) wird in die Ansteuerschaltung 12 eingespeist.
Zusätzlich kann der Bewegungswinkel θ(n + 1) des Gelenkstückes zu dem Zeitpunkt t(n + 1) nach Ablauf von Δt von dem Zeitpunkt "tn" erhalten werden durch Hinzuzählen des Produkts der Winkelgeschwindigkeit V(n + 1) zu dem Zeitpunkt t(n + 1) und Δt zu dem Bewegungswinkel θ(n) des Gelenkstückes zu dem Zeitpunkt t(n).
Während des Zeitintervalls zwischen dem Zeitpunkt des Bewegungsbeginns und dem Zeitpunkt des Bewegungsendes kann der Winkel jedes Gelenkstückes nach jedem Ablauf der Abtastperiode Δt in der folgenden Weise berechnet werden. Jedes Mal wenn eine Abtastperiode Δt abgelaufen ist nach dem Zeitpunkt des Bewegungsbeginns wird die Winkelgeschwindigkeit jedes Gelenkstückes zu einem nächsten Abtastzeitpunkt (einem Zeitpunkt nach Ablauf des Zeitintervalls Δt von dem gegenwärtigen Zeitpunkt) gemäß dem Geschwindigkeitsmuster berechnet. Die berechnete Geschwindigkeit wird mit der Abtastperiode Δt multipliziert. Der vorhergehende und der letzte Multiplikationsergebniswert werden addiert. Das Ergebnis der Addition zeigt den Winkel des Verbindungsstückes nach jedem Ablauf der Abtastperiode Δt an. Information des berechneten Winkels des Gelenkstückes wird in die Ansteuerschaltung 12 eingegeben.
Es sei bemerkt, dass, wie man eine Winkelgeschwindigkeit und eine Position (Ort) jedes Gelenkstückes des Roboterkörpers 1 berechnet, z. B. in der US-Patentveröffentlichung US 6,127,792 enthalten ist. Da das US-Patent dem gleichen Inhaber wie dem dieser Anmeldung zugeordnet ist, werden Offenbarungen davon hierin durch Inbezugnahme mit aufgenommen.
Das Programmierhandgerät 3 ist als ein Handgerät ausgestaltet, das aus einer Eingabeeinheit, einer Anzeigeeinheit, einer Speichereinheit und einer Recheneinheit be steht, welche die Ausführung von grundlegenden Funktionen ermöglichen, wie z. B. einer Programmstartfunktion, Trajektorien-Teach-In-Funktion, Maschinensperrfunktionen, Fehlermeldungsanzeigefunktionen und dergleichen. Diese tragbare Konfiguration des Programmierhandgerätes 3 ermöglicht es einem Bediener, den Roboterkörper 1 durch das Steuergerät 2 zu steuern, während er die Bewegung des Roboterkörpers 1 beobachtet.
Bei der Ausführungsform wird insbesondere Information, die eine gewünschte Trajektorie des einen Endes des Roboterkörpers 1 angibt, an das Steuergerät 3 durch Programmierprozesse des Programmierhandgerätes 3 übergeben.
Die Programmierprozesse beinhalten einen Prozess des dem Steuergerät 2 Beibringens (Teach-In) einer Mehrzahl von Anweisungspunkten des einen Endes des Manipulators R; wobei diese Anweisungspunkte durch die gewünschte Trajektorie verlaufen und einen Startpunkt, einen Endpunkt und Zwischenpunkte dazwischen beinhalten.
Die Programmierprozesse beinhalten auch einen Prozess des Einprogrammierens einer gewünschten Stellung des einen Endes des Roboterkörpers 1 bei jedem der Anweisungspunkte.
Das Steuergerät 2 empfängt die einprogrammierten Anweisungspunkte und die gewünschte Stellung des einen Endes des Roboterkörpers 1 an jedem der Anweisungspunkte und speichert diese in dem RAM 15.
Beim tatsächlichen In-Bewegung-Bringen des Roboterkörpers 1 ist das Steuergerät 2 derart programmiert, dass es:
Interpolation auf den einprogrammierten Anweisungspunkten ausführt, um eine durch die Anweisungspunkte in der Reihenfolge von dem Startpunkt zu dem Endpunkt verlaufende Trajektorie zu bestimmen; und
das eine Ende des Roboterkörpers 1 entlang der bestimmten Trajektorie durchgehend bewegt.
Es sei bemerkt, dass bei der wie in 1 dargestellten Ausführungsform ein Roboterkoordinatensystem (X, Y, Z) unabhängig von der Bewegung des einen Endes des Roboterkörpers 1 eingeführt ist.
Zum Beispiel ist das Roboterkoordinatensystem (X, Y, Z) so eingeführt, dass die Z-Achse davon mit der Mittenachse der Basis 4 zusammenfällt.
Darüber hinaus ist bei der Ausführungsform ein festes dreidimensionales Koordinatensystem unabhängig von der Bewegung des einen Endes des Roboterkörpers 1, in anderen Worten der daran befestigten Hand, eingeführt. Der Ursprung des dreidimensionalen Koordinatensystems, das als ein "Handkoordinatensystem (x_H, y_H, z_H)" bezeichnet werden wird, entspricht der Mittenposition des spitzenseitigen Endes 9a des Flansches 9.
Insbesondere wird die Position des einen Endes des Roboterkörpers 1 dadurch wiedergegeben, wo der Ursprung des Handkoordinatensystems (x_H, y_H, z_H) (die Mittenposition des spitzenseitigen Endes 9a des Flansches 9) sich in dem Roboterkoordinatensystem (X, Y, Z) befindet.
Darüber hinaus ist in dem Roboterkoordinatensystem die Stellung des einen Endes des Roboterkörpers 1 definiert durch Orientierungen der Einheitsvektoren auf vorbestimmten zwei Achsen der drei Achsen des Handkoordinatensystems (x_H, y_H, z_H). Das Steuergerät ist außerdem derart funktionsfähig, dass es Interpolation auf den Stellungen des einen Endes des Roboterkörpers 1 an den Anweisungspunkten ausführt, um eine Ortslinie zu bestimmen, die den Stellungen des einen Endes des Roboterkörpers 1 an den Anweisungspunkten entspricht; und
dass es das eine Ende des Roboterkörpers 1 in glatter Art und Weise so bewegt, dass sich die Stellung des einen Endes des Roboterkörpers 1 entlang der bestimmten Ortslinie in glatter Weise ändert.
Bei der Ausführungsform wird als Verfahren des Interpolierens auf der Mehrzahl von Anweisungspunkten ein Spline-Interpolationsverfahren verwendet, das Spline-Funktionen anwendet. Ein Beispiel des Spline-Interpolationsverfahrens gemäß der Ausführungsform wird mit Bezug auf 4 beschrieben werden.
In 4 stellen R0, R1, ..., R(k – 1), Rk, R(k + 1), ..., Rn – 1, Rn jeweils eine Anzahl (n + 1) von Anweisungspunkten dar, R0 stellt den Startpunkt dar und Rn stellt den Endpunkt dar. Es sei bemerkt, dass "n" eine ganze Zahl gleich oder mehr als 2 und "k" eine ganze Zahl gleich oder mehr als 1 und weniger als n ist.
Zusätzlich stellen Kurven G1, G2, ..., G(k – 1), Gk, G(k + 1), ..., Gn – 1, Gn, von denen jede zwischen einem durch entsprechende benachbarte Anweisungspunkte gebildeten Abschnitt verbindet, Spline-Kurvensegmente dar. Die Spline-Kurvensegmente G1 bis Gn sind derart definiert, dass sie entsprechende Abschnitte jeweils durch verschiedene Polynome interpolieren. Das Spline-Interpolationsverfahren definiert eine Spline-Kurve durch eine Spline-Kurvensegmentfunktion, die jedem der Spline-Kurvensegmente entspricht.
Insbesondere kann jeder Punkt auf jedem der Spline-Kurvensegmente definiert werden als ein Punkt in dem dreidimensionalen Roboterkoordinatensystem (X, Y, Z) durch die folgenden kubischen Polynome [1] bis [3]: X(t) = a3t3 + a2t2 + a1t + a [1] Y(t) = b3t3 + b2t2 + b1t + b [2] Z(t) = c3t3 + c2t2 + c1t + c [3]wobei t ein auf einen Bereich von 0 bis 1 festgelegter Parameter ist.
Kombinieren der kubischen Polynome [1] bis [3] ermöglicht daher, dass eine Position jedes Punktes auf jedem Spline-Kurvensegment durch das folgende kubische allgemeine Polynom (Spline-Kurvensegmentfunktion) [4] definiert ist: P →(t) = A →t3 + B →t2 + C →t + D → [4]wobei P → eine Position auf jedem Spline-Kurvensegment darstellt, A →, B →, C →, D → jeweils Vektorkoeffizienten darstellen.
Durch Lösen der Vektorkoeffizienten A →, B →, C →, D → des Spline-Kurvensegmentes Gk können die folgenden Gleichungen [9] bis [12] erhalten werden.
Insbesondere, da eine Position eines startpunktseitigen Anweisungspunktes des k-ten Spline-Kurvensegmentes Gk als "R(k – 1)" angenommen werden kann und der des endpunktseitigen Anweisungspunktes davon als "R(k)" angenommen werden kann, können Positionsvektoren beider Enden des k-ten Spline-Kurvensegmentes Gk durch P →(k – 1) und P → k wiedergegeben werden.
Dann sollten die Positionsvektoren P → (k – 1) und P → (k) der beiden Enden des Spline-Kurvensegmentes Gk die folgenden Gleichungen erfüllen: P →(0) = A →0 = P →(k – 1) [5] P →(1) = A → + B → + C → + D → = P →k [6]wobei 0 ≤ t ≤ 1.
Zusätzlich sind die Tangentenvektoren an der Position R(k – 1) an einem startpunktseitigen Anweisungspunkt und der Position R(k) an dem endpunktseitigen Anweisungspunkt durch P →'(t) = 3A →t² + 2B →t + C → gegeben, das erhalten werden kann durch Differenzieren der Gleichung [4] nach t. Unter der Annahme, dass die Tangentenvektoren an der Position R(k – 1) an einem startpunktseitigen Anweisungspunkt und der Position R(k) an dem endpunktseitigen Anweisungspunkt wiedergegeben werden durch T →(k – 1) und T →k, werden die folgenden Gleichungen aufgestellt: P →'(0) = A → = T →(k – 1) [7] P →'(1)=3A →+2B →+ C → = T →k [8]
Die obigen Gleichungen [5] bis [8] ermöglichen, dass die Vektorkoeffizienten A →, B →, C →, D → der Spline-Kurve durch die folgenden Gleichungen [9] bis [12] wiedergegeben werden: A → = 2P →(k – 1) – 2P →k + T →(k – 1) + T →(k) [9] B → = 3P →k – 3P →(k – 1) – 2T →(k – 1) – T →(k) [10] C → = T →(k – 1) [11] D → = P →(k – 1) [12]
Die Gleichungen [9] bis [12] ermöglichen, dass die Gleichung [4] durch die folgende allgemeine Gleichung [13] wiedergegeben wird: P →(t) = {2(P →(k – 1)t + T →(k – 1) + T →k}·t3 + {{3(P →k – P →(k – 1)t – 2T →(k – 1) – T →k}·t2 + T →(k – 1)·t + P →(k – 1) [13]
Wie oben beschrieben, wenn ein Abschnitt als ein Intervall zwischen benachbarten Anweisungspunkten definiert ist, wird das Spline-Kurvensegment in jedem Abschnitt durch die allgemeine Gleichung [13] wiedergegeben.
Insbesondere werden die Spline-Kurvensegmente so festgelegt, dass die Tangentenvektoren T →(k – 1) und T →k die folgende erste bis dritte Bedingung erfüllen:
Die erste Bedingung ist es, dass die Positionen an den einen Enden der benachbarten Spline-Kurvensegmente miteinander übereinstimmen; wobei diese einen Enden einen Knoten zwischen den benachbarten Spline-Kurvensegmenten bilden.
Die zweite Bedingung ist es, dass:
die Größe (Länge) jedes der Tangentenvektoren an beiden Enden (Position des startpunktseitigen Anweisungspunktes und der eines endpunktseitigen Anweisungspunktes) eines Spline-Kurvensegmentes derart festgelegt wird, dass sie proportional zu der Länge einer ersten geraden Linie ist, welche die beiden Enden des einen Spline-Kurvensegmentes verbindet,
die Tangentenvektoren an beiden Enden des einen Spline-Kurvensegmentes derart ausgerichtet sind, dass sie einen ersten Winkel und einen zweiten Winkel jeweils im wesentlichen halbieren; wobei dieser erste Winkel durch die erste gerade Linie und eine zweite gerade Linie, welche die beiden Enden des vorhergehenden Spline-Kurvensegmentes verbindet, gebildet wird, und wobei dieser zweite Winkel durch die erste gerade Linie und eine dritte gerade Linie, welche die beiden Enden des nächsten Spline-Kurvensegmentes verbindet, gebildet wird.
Die dritte Bedingung ist es, dass die Richtung eines Tangentenvektors an einer Position eines startpunktseitigen Anweisungspunktes eines Spline-Kurvensegmentes im wesentlichen gleich der eines Tangentenvektors an einer Position eines endpunktseitigen Anweisungspunktes des vorhergehenden Spline-Kurvensegmentes und gleich der eines Tangentenvektors an einer Position des startpunktseitigen Anweisungspunktes des nächsten Spline-Kurvensegmentes ist.
Als nächstes werden die erste bis dritte Bedingung im Detail beschrieben werden durch Aufgreifen des zweiten Spline-Kurvensegmentes (k = 2; ein Abschnitt zwischen dem startpunktseitigen Anweisungspunkt R1 und dem endpunktseitigen Anweisungs punkt R2). Es sei bemerkt, dass 5 den Bereich zwischen dem Anweisungspunkt R3 und dem Anweisungspunkt R1 vergrößert und ihn darstellt.
In 5 wird die gerade Linie, welche zwischen den beiden Enden (den Anweisungspunkten R0 und R1) des ersten Spline-Kurvensegmentes (k = 1) verbindet, durch L1 wiedergegeben, und wird die gerade Linie, die zwischen den beiden Enden (den Anweisungspunkten R1 und R2) des zweiten Spline-Kurvensegmentes (k = 2) verbindet, durch L2 wiedergegeben. Ebenso wird die gerade Linie, die zwischen den beiden Enden (den Anweisungspunkten R2 und R3) des zweiten Spline-Kurvensegmentes (k = 2) verbindet, durch L3 wiedergegeben.
Die Tangentenvektoren an beiden Enden (den startpunktseitigen und endpunktseitigen Anweisungspunkten R0 und R1) des ersten Spline-Kurvensegmentes G1 werden wiedergegeben durch einen startpunktseitigen Tangentenvektor T →1s und einem endpunktseitigen Tangentenvektor T →1e.
Ebenso werden die Tangentenvektoren an beiden Enden (den startpunktseitigen und endpunktseitigen Anweisungspunkten R1 und R2) des zweiten Spline-Kurvensegmentes G2 wiedergegeben durch einen startpunktseitigen Tangentenvektor T →2s und einem endpunktseitigen Tangentenvektor T →2e. Ferner werden die Tangentenvektoren an beiden Enden (den startpunktseitigen und endpunktseitigen Anweisungspunkten R2 und R3) des dritten Spline-Kurvensegmentes G3 wiedergegeben durch einen startpunktseitigen Tangentenvektor T →3s und einen endpunktseitigen Tangentenvektor T →3e.
Bei der Ausführungsform können der endpunktseitige Tangentenvektor T →1e des ersten Spline-Kurvensegmentes G1, der startpunktseitige Tangentenvektor T →2s des zweiten Spline-Kurvensegmentes G2 und der endpunktseitige Tangentenvektor T →2e des zweiten Spline-Kurvensegmentes G2 durch die folgenden Gleichungen [14] bis [16] wiedergegeben werden. Ebenso können der startpunktseitige Tangentenvektor T →3s des dritten Spline-Kurvensegmentes G3 und der endpunktseitige Tangentenvektor T →3e des dritten Spline-Kurvensegmentes G3 durch die folgenden Gleichungen [17] und [18] wiedergegeben werden:
Es sei bemerkt, dass die Größe des startpunktseitigen Tangentenvektors T →1s, da es kein vorhergehendes Spline-Kurvensegment des startpunktseitigen Tangentenvektors T →1s des ersten Spline-Kurvensegmentes G1 gibt, auf das Produkt der Länge der ersten geraden Linie L1 und einer Proportionalitätskonstante "r" festgelegt wird. Die Richtung des startpunktseitigen Tangentenvektors T →1s wird derart festgelegt, dass sie gleich der der ersten geraden Linie L1 ist.
In den Gleichungen [14] bis [18] ist "r" eine Proportionalitätskonstante, dessen Wert gleich dem der Proportionalitätskonstante "r" ist, die beim Erlangen der Größe des startpunktseitigen Tangentenvektors T →1s verwendet wird. Bei der Ausführungsform wird der Wert der Proportionalitätskonstante "r" auf "1" festgesetzt.
In den Gleichungen [14] bis [18] sind die Symbole P →0, P →1, P →2, P →3 jeweils mit einem Richtungspfeil Positionsvektoren an den Anweisungspunkten R0, R1, R2 bzw. R3.
Aus diesem Grunde gibt in den Gleichungen [14] und [15] der erste Term innerhalb der Klammer einen Einheitsvektor L →1 wieder, dessen Richtung gleich der der ersten geraden Linie L1 ist, und der zweite Term darin gibt einen Einheitsvektor L →2 wieder, dessen Richtung gleich der der zweiten geraden Linie L2 ist.
Ebenso gibt in den Gleichungen [16] und [17] der erste Term innerhalb den Klammern einen Einheitsvektor L →2 wieder, dessen Richtung die gleiche wie die der zweiten geraden Linie L2 ist, und der zweite Term darin einen Einheitsvektor L →3 wieder, dessen Richtung gleich der der dritten geraden Linie L3 ist.
Die Berechnung der Gleichungen innerhalb der Klammern in den Gleichungen [14] und [15] ermöglicht es, die Summe der Einheitsvektoren L →1 und des Einheitsvektors L →2 zu erhalten. Wie in 6 dargestellt gibt die Summe des Einheitsvektors L →1 und des Einheitsvektors L →2 einen Einheitsvektor b →1 auf der Winkelhalbierenden eines Winkels α wieder, der durch eine Verlängerung der ersten geraden Linie L1 und der zweiten geraden Linie L2 gebildet ist. Multiplikation des Einheitsvektors b →1 mit einer Effektiv-Zahl, wie z. B. 1/2 (0,5) lässt es zu, die Winkelhalbierende des Winkels α, der durch die Verlängerung der ersten geraden Linie L1 und der zweiten geraden Linie L2 gebildet wird, zu berechnen.
Ebenso ermöglicht die Berechnung der Gleichungen innerhalb den Klammern in den Gleichungen [16] und [17], dass die Summe des Einheitsvektors L →2 und des Einheitsvektors L →3 erhalten wird. Wie in 7 dargestellt, gibt die Summe des Einheitsvektors L →2 und des Einheitsvektors L →3 einen zu berechnenden Einheitsvektor b →2 auf der Winkelhalbierenden eines Winkels β wieder, der durch eine Verlängerung der zweiten geraden Linie L2 und der dritten geraden Linie L3 gebildet ist. Multiplikation des Einheitsvektors b →2 mit einer Effektiv-Zahl, wie z. B. 1/2 (0,5) erlaubt es, die Winkelhalbierende des durch die Verlängerung der zweiten geraden Linie L2 und der dritten geraden Linie L3 gebildeten Winkels β zu berechnen.
Wie oben ausgeführt, ermöglichen Berechnungen unter Verwendung der Gleichungen [14] bis [18], den startpunktseitigen Tangentenvektor und den endpunktseitigen Tangentenvektor für jedes der Spline-Kurvensegmente zu erhalten.
Danach werden, wie oben ausgeführt, die vier Spline-Parameter A →, B →, C →, D → jedes der Spline-Kurvensegmente erhalten unter Verwendung des startpunktseitigen Tangentenvektors und des endpunktseitigen Tangentenvektors jedes der Spline-Kurvensegmente. Somit kann die Spline-Kurvensegmentfunktion (kubisches allgemeines Polynom) mit den folgenden Eigenschaften erhalten werden:
Die erste Eigenschaft ist es, dass die Richtung des startpunktseitigen Tangentenvektors jedes der Spline-Kurvensegmente im wesentlichen gleich der des endpunktseitigen Tangentenvektors des vorhergehenden Spline-Kurvensegmentes ist.
Die zweite Eigenschaft ist es, dass die Richtung des endpunktseitigen Tangentenvektors jedes der Spline-Kurvensegmente im wesentlichen die gleiche wie die des startpunktseitigen Tangentenvektors des nächsten Spline-Kurvensegmentes ist.
Dies ermöglicht, die Spline-Kurve (Spline-Funktion) in glatter Art und Weise an jedem der Anweisungspunkte (Knoten) verlaufend zu erhalten.
Als nächstes werden Verfahrensschritte des Steuergerätes 2 zum Bewirken, dass der Roboterkörper 1 verschiedene Arbeiten ausführt, im Folgenden beschrieben werden.
Bevor dem Bewirken, dass der Roboter R verschiedene Aufgaben ausführt, verwendet ein Bediener das Programmierhandgerät 3, um den Roboterkörper 1 tatsächlich unter der Steuerung des Steuergerätes 2 zu bewegen, wodurch dem Steuergerät 2 die Anweisungspunkte des einen Endes (spitzenseitiges Ende 9a des Flansches 9) des Roboterkörpers 1 und eine gewünschte Stellung des einen Endes des Roboterkörpers 1 an jedem der Anweisungspunkte beigebracht bzw. einprogrammiert werden. Die Anwei sungspunkte werden festgelegt für eine gewünschte Trajektorie und beinhalten einen Startpunkt, einen Endpunkt und Zwischenpunkte. Die durch das Programmierhandgerät 3 einprogrammierten Anweisungspunkte werden in dem RAM 15 von der CPU 11 gespeichert.
Es sei bemerkt, dass mit der Steuerung der Stellung an jedem Anweisungspunkt verknüpfte Beschreibungen ausgelassen werden, um die Beschreibungen der Verfahrensschritte des Steuergerätes 2 zu vereinfachen.
Wenn eine Startanweisung von dem Programmierhandgerät 3 an das Steuergerät 2 eingegeben wird, liest zum Beispiel die CPU 11 des Steuergerätes 2 die Anweisungspunkte von dem RAM 15 im Schritt S1 aus und bestimmt Tangentenvektoren an dem Startpunkt und diejenigen an dem Endpunkt in Schritt S2.
Bei dieser Bestimmung in Schritt S2 wird der Tangentenvektor an dem Startpunkt derart festgelegt, dass er die Richtung besitzt, die gleich der einer geraden Linie ist, welche zwischen dem Startpunkt und dem nächsten Anweisungspunkt verbindet, und dass er die Länge proportional zu der der geraden Linie aufweist. Ebenso wird der Tangentenvektor an dem Endpunkt derart festgelegt, dass er die gleiche Richtung wie die der geraden Linie besitzt, die zwischen dem Endpunkt und dem vorhergehenden Anweisungspunkt verbindet, und dass er die Länge proportional zu der der geraden Linie aufweist. Es sei bemerkt, dass die Richtung und Länge der Tangentenvektoren an sowohl dem Startpunkt als auch dem Endpunkt nicht beschränkt sind auf diese zuvor ausgeführten. Insbesondere können die Richtung und Länge der Tangentenvektoren an sowohl dem Startpunkt als auch dem Endpunkt bestimmt werden mit Berücksichtigung der Richtungen und Längen der übrigen Tangentenvektoren. Gemäß den Umständen kann die Länge der Tangentenvektoren an sowohl dem Startpunkt als auch an dem Endpunkt auf Null festgesetzt werden.
Als nächstes teilt die CPU 11 des Steuergerätes 2 den Bereich zwischen dem Startpunkt und dem Endpunkt an den Zwischenpunkten in eine Mehrzahl von Ab schnitten (Segmente) und berechnet den startpunktseitigen Tangentenvektor und den endpunktseitigen Tangentenvektor für jeden der Abschnitte gemäß den Gleichungen (14) bis (18) in Schritt S3.
Anschließend erhält die CPU 11 die Vektorkoeffizienten A →, B →, C →, D → des kubischen allgemeinen Polynoms (Spline-Kurvenfunktion; siehe Gleichung (4)), die ein Spline-Kurvensegment jedes der Abschnitte gemäß den Gleichungen (9) bis (12) in Schritt S4 definieren.
Das kubische allgemeine Polynom (Spline-Kurvenfunktion) und die erhaltenen Vektorkoeffizienten A →, B →, C →, D → stellen die Spline-Kurve bereit, die alle Anweisungspunkte von ihrem Startpunkt bis zu ihrem Endpunkt in glatter Art und Weise verbindet.
Danach differenziert die CPU 11 die Gleichung (13) nach "t" (0 ≤ t ≤ 1), um eine Gleichung zu erhalten, und integriert die erhaltene Gleichung von 0 bis 1, wodurch die gesamte Wegstrecke der Spline-Kurve von dem Startpunkt zu dem Endpunkt in Schritt S5 erhalten wird.
Da die erhaltene gesamte Wegstrecke der Spline-Kurve die Wegstrecke des einen Endes des Roboterkörpers 1 von dem Startpunkt zu dem Endpunkt bedeutet, erzeugt die CPU 11 das durch 3(a) veranschaulichte trapezoide Geschwindigkeitsmuster in Schritt S6.
Wie oben ausgeführt ist das trapezoide Geschwindigkeitsmuster so definiert, dass die Ortslinie der Geschwindigkeitsänderung des einen Endes des Roboterkörpers 1 eine im wesentlichen trapezoide Form aufweist. Insbesondere besteht das trapezoide Geschwindigkeitsmuster aus
einem ersten Abschnitt, der einer Beschleunigungsphase während einer Zeitspanne T1 entspricht;
einem zweiten Abschnitt, der einer Konstantgeschwindigkeitsphase mit einer maximalen Geschwindigkeit während einer Zeitspanne T2 entspricht; und
einem dritten Abschnitt, der einer Abbremsungsphase T3 während einer Zeitspanne T3 entspricht.
Danach berechnet die CPU 11 die Bewegungsstrecke des einen Endes des Roboterkörpers 1 innerhalb jeder konstanten Abtastperiode auf der Grundlage des trapezoiden Geschwindigkeitsmusters in Schritt S7. Zusätzlich addiert die CPU 11 in Schritt 7 die Bewegungsstrecke innerhalb einer gegenwärtigen Abtastperiode zu der innerhalb der vorhergehenden Abtastperiode, um die Endposition der Bewegung des einen Endes des Roboterkörpers 1 auf der Spline-Kurve innerhalb der gegenwärtigen Abtastperiode zu erhalten. Darüber hinaus berechnet die CPU 11 in Schritt S7 die Stellung des einen Endes des Roboterkörpers 1.
Als nächstes berechnet die CPU 11 die Winkelgeschwindigkeit und den Ort jedes Gelenkstückes des Roboterkörpers 1, das zum Bewegen des einen Endes des Roboterarms von dessen Bewegungsstartposition zu dessen Endposition der Bewegung benötigt wird, innerhalb der entsprechenden Abtastperiode in Schritt S8. Anschließend gibt die CPU 11 die berechnete Winkelgeschwindigkeit und den Ort jedes Gelenkstückes des Roboters 1 als den Winkelgeschwindigkeitsanweisungswert und den Positionsanweisungswert an die Ansteuerschaltung 12 in Schritt S9 aus.
Als nächstes bestimmt die CPU 11, ob die Endposition des einen Endes des Roboterkörpers 1 den Endpunkt erreicht, der von dem Bediener als der Anweisungspunkt eingegeben ist.
Wenn bestimmt ist, dass die Endposition des einen Endes des Roboterkörpers 1 nicht den von dem Bediener als den Anweisungspunkt eingegebenen Endpunkt erreicht (die Bestimmung im Schritt S10 ist NEIN), kehrt die CPU 11 zu Schritt S7 zurück. Dann führt die CPU 11 wiederholt die Prozesse aus Schritten S7 bis S11 aus bis be stimmt ist, dass die Endposition des einen Endes des Roboterkörpers 1 den eingegebenen Endpunkt erreicht.
Wenn somit bestimmt ist, dass die Endposition des einen Endes des Roboterkörpers 1 den von dem Bediener als den Anweisungspunkt eingegebenen Endpunkt erreicht (die Bestimmung in Schritt S10 ist JA), bestimmt die CPU 11, dass das eine Ende des Roboterkörpers 1 den Endpunkt vollständig erreicht hat.
Wie oben beschrieben werden gemäß der Ausführungsform, um eine glatte Spline-Kurve (Trajektorie) ohne Schlingenabschnitte zu erhalten, die Spline-Kurvensegmente so bestimmt, dass die Tangentenvektoren beider Enden jedes Spline-Kurvensegmentes Bedingungen erfüllen, die mit ihren Richtungen verknüpft sind, unabhängig von den folgenden Bedingungen:
Die Tangentenvektoren an den einen Enden der benachbarten Spline-Kurvensegmente stimmen miteinander überein.
Die Steigungen der Tangentenvektoren an den einen Enden der benachbarten Spline-Kurvensegmente stimmen miteinander überein.
Das bedeutet, dass der Erfinder der vorliegenden Erfindung herausgefunden hat, dass das Auftreten von Schleifenabschnitten wahrscheinlich den eben ausgeführten Bedingungen zugeordnet werden kann.
Somit ermöglicht bei der Ausführungsform der vorliegenden Erfindung die Einführung von neuen Bedingungen, die verknüpft sind mit deren Richtungen, unabhängig von den mit dem Zusammenfallen der benachbarten Spline-Kurvensegmente selbst, und deren Steigungen verknüpften Bedingungen, eine glatte Spline-Kurve (Trajektorie) ohne Schlaufenabschnitte zu erhalten.
Insbesondere ist gemäß der Ausführungsform das Steuergerät 2 derart ausgebildet, dass es den Tangentenvektor an beiden Enden (Anweisungspunkten) eines Spline-Kurvensegmentes jedes Abschnittes so bestimmt, dass:
die Größe jedes der Tangentenvektoren an beiden Enden eines Spline-Kurvensegmentes proportional zu der Länge einer ersten geraden Linie ist, welche die beiden Enden eines Spline-Kurvensegmentes verbindet;
der Tangentenvektor an dem einen Ende des einen Spline-Kurvensegmentes derart ausgerichtet ist, dass er einen ersten Winkel halbiert, der durch die erste gerade Linie und eine Verlängerung einer beide Enden des vorhergehenden Spline-Kurvensegmentes verbindenden zweiten geraden Linie gebildet ist; und
der Tangentenvektor an dem anderen Ende des einen Spline-Kurvensegmentes so ausgerichtet ist, dass er einen zweiten Winkel halbiert, der durch eine Verlängerung der ersten geraden Linie und eine beide Enden des nächsten Spline-Kurvensegmentes verbindende dritte gerade Linie gebildet ist.
Dies kann das Auftreten von Schlaufenabschnitten bei der erzeugten Spline-Kurve (Trajektorie) verhindern, selbst wenn die Anweisungspunkte in der Länge voneinander verschieden sind.
Dies ist deshalb so, da:

(1) der startpunktseitige Tangentenvektor des einen anfangsseitigen Endes jedes Spline-Kurvensegmentes so festgelegt ist, dass er näher zu dem endpunktseitigen Ende jedes Spline-Kurvensegmentes gerichtet ist, und
(2) benachbarte Spline-Kurvensegmente ähnlich zueinander sind, da die Größen des startpunktseitigen Tangentenvektors und des endpunktseitigen Tangentenvektors jedes Spline-Kurvensegmentes identisch zueinander sind und proportional zu der Größe einer geraden Linie sind, die beide Enden jedes Spline-Kurvensegmentes verbindet, wenn ein durch den startpunktseitigen Tangentenvektor jedes Spline-Kurvensegmentes und eine Verlängerung einer beide Enden des vorhergehenden Spline-Kurvensegmentes verbindenden geraden Linie gebildeter Winkel identisch ist zu dem, der durch den startpunktseitigen Tangentenvektor und eine beide Enden jedes Spline-Kurvensegmentes verbindenden geraden Linie gebildet ist.

Es ist möglich, mathematisch nachzuweisen, dass die Ausführungsform der vorliegenden Erfindung das Erhalten einer Spline-Kurve ohne Schlaufenabschnitte ermöglicht.
Bei der Ausführungsform werden die Tangentenvektoren an beiden Enden (Anweisungspunkten) eines Spline-Kurvensegmentes jedes Abschnittes so bestimmt, dass:
der Tangentenvektor an dem einen Ende des einen Spline-Kurvensegmentes derart ausgerichtet ist, dass er einen ersten Winkel halbiert, wobei dieser erste Winkel durch die erste gerade Linie und eine Verlängerung einer beide Enden des vorhergehenden Spline-Kurvensegmentes verbindenden zweiten geraden Linie gebildet ist; und
der Tangentenvektor an dem anderen Ende des einen Spline-Kurvensegmentes derart ausgerichtet ist, dass er einen zweiten Winkel halbiert, wobei dieser zweite Winkel durch eine Verlängerung der ersten geraden Linie und eine beide Enden des nächsten Spline-Kurvensegmentes verbindenden dritten geraden Linie gebildet ist.
Jedoch ist die vorliegende Erfindung nicht auf die Struktur beschränkt.
Insbesondere können die Tangentenvektoren an beiden Enden (Anweisungspunkte) eines Spline-Kurvensegmentes jedes Abschnittes so bestimmt werden, dass:
der Tangentenvektor an dem einen Ende des einen Spline-Kurvensegmentes derart ausgerichtet ist, dass er einen ersten Winkel in einem vorbestimmten Verhältnis teilt; wobei dieser erste Winkel durch die erste gerade Linie und eine Verlängerung einer beide Enden des vorhergehenden Spline-Kurvensegmentes verbindenden zweiten geraden Linie gebildet ist; und
der Tangentenvektor an dem anderen Ende des einen Spline-Kurvensegmentes derart ausgerichtet ist, dass er einen zweiten Winkel in einem vorbestimmten Verhältnis teilt; wobei dieser zweite Winkel durch eine Verlängerung der ersten geraden Linie und eine beide Enden des nächsten Spline-Kurvensegmentes verbindenden dritten geraden Linie gebildet ist.
Bei der Ausführungsform ist die Proportionalitätskonstante "r" nicht beschränkt auf "1". Es ist möglich, zu verhindern, dass eine Spline-Kurve Schlaufenabschnitte enthält, vorausgesetzt dass die Proportionalitätskonstante "r" innerhalb eines vorbestimmten Bereiches ist.
Die vorliegende Erfindung kann angewendet werden auf verschiedene Typen von Robotern ohne Beschränkung eines solchen Vertikalgelenkroboters.
Zusätzlich werden die Fachleute würdigen, dass die vorliegende Erfindung als Programmprodukt verteilt werden kann, wobei die Programme z.B. in dem RAM 15 in einer Formenvielfalt gespeichert sind. Es ist auch wichtig zu bemerken, dass die vorliegende Erfindung gleichermaßen gilt unabhängig von der besonderen Art der signaltragenden Medien, die zum tatsächlichen Ausführen der Verteilung verwendet werden. Beispiele für geeignete signaltragende Medien sind beschreibbare Medienarten wie z.B. CD-ROMs und DVD-ROMs sowie Medien vom Übertragungs-Typ wie z.B. digitale und analoge Kommunikationsverbindungen.
Während beschrieben worden ist, was zur Zeit als die Ausführungsformen und ihre Abwandlungen der vorliegenden Erfindung betrachtet wird, wird verstanden wer den, dass verschiedene Abwandlungen, die noch nicht beschrieben sind, dabei gemacht werden können, und es ist beabsichtigt, in den angehängten Ansprüchen alle solche Abwandlungen abzudecken, die unter die wahre Idee und in den Umfang der Erfindung fallen.

Claims

Steuergerät zum Bestimmen einer Spline-Kurve als eine Trajektorie eines vorbestimmten beweglichen Abschnittes eines Roboterkörpers, wobei das Steuergerät umfasst: eine Interpoliereinheit, die derart ausgebildet ist, dass sie nacheinander Spline-Kurvensegmente jeweils in Abschnitten, die Intervallen zwischen Anweisungspunkten entsprechen, interpoliert; eine Tangentenvektorbestimmungseinheit, die derart ausgebildet ist, dass sie Tangentenvektoren an dem einen und dem anderen Ende eines Spline-Kurvensegmentes von jedem der Abschnitte so bestimmt, dass: eine Länge jedes der Tangentenvektoren proportional zu der einer das eine und das andere Ende des einen Spline-Kurvensegmentes verbindenden ersten geraden Linie ist; und die Tangentenvektoren an dem einen Ende und dem anderen Ende des einen Spline-Kurvensegmentes derart ausgerichtet sind, dass sie einen ersten Winkel und einen zweiten Winkel jeweils in einem vorbestimmten Verhältnis teilen, wobei der erste Winkel durch die erste gerade Linie und eine Verlängerung einer das eine und das andere Ende eines vorhergehenden Spline-Kurvensegmentes verbindenden zweiten geraden Linie gebildet ist, und wobei der zweite Winkel durch die erste gerade Linie und eine Verlängerung einer das eine und das andere Ende eines nächsten Spline-Kurvensegmentes verbindenden dritten geraden Linie gebildet ist; und eine Verbindungseinheit, die derart ausgebildet ist, dass sie die Spline-Kurvensegmente derart verbindet, dass: eine Richtung eines der Tangentenvektoren an dem einen Ende des einen Spline-Kurvensegmentes im wesentlichen gleich der eines der Tangentenvektoren an dem gleichen Ende des vorhergehenden Spline-Kurvensegmentes ist; und eine Richtung des anderen der Tangentenvektoren an dem anderen Ende des einen Spline-Kurvensegmentes im wesentlichen die gleiche wie die des einen der Tangentenvektoren an dem gleichen Ende des nächsten Spline-Kurvensegmentes ist.
Steuergerät zum sequentiellen Interpolieren von Spline-Kurvensegmenten gemäß Anspruch 1, wobei die Tangentenvektoren an dem einen Ende und an dem anderen Ende des einen Spline-Kurvensegmentes derart ausgerichtet sind, dass sie den ersten Winkel und den zweiten Winkel jeweils halbieren.
Programmprodukt, dass in einen Speicher eines Steuergerätes zum Bestimmen einer Spline-Kurve als eine Trajektorie eines vorbestimmten beweglichen Abschnittes eines Roboterkörpers eingebettet ist, wobei das Programmprodukt umfasst: ein erstes Mittel zum Anweisen eines Steuergerätes, Spline-Kurvensegmente jeweils in Abschnitten sequentiell zu interpolieren, die Intervallen zwischen Anweisungspunkten entsprechen; ein zweites Mittel zum Anweisen eines Steuergerätes, Tangentenvektoren an einem und an dem anderen Ende des einen Spline-Kurvensegmentes jedes der Abschnitte so zu bestimmen, dass: eine Länge jedes der Tangentenvektoren proportional zu der einer das eine und das andere Ende des einen Spline-Kurvensegmentes verbindenden ersten geraden Linie ist; und die Tangentenvektoren an dem einen und an dem anderen Ende des einen Spline-Kurvensegmentes derart ausgerichtet sind, dass sie einen ersten Winkel und einen zweiten Winkel jeweils in einem vorbestimmten Verhältnis teilen, wobei der erste Winkel durch die erste gerade Linie und eine Verlängerung einer das eine und das andere Ende eines vorhergehenden Spline-Kurvensegmentes verbindenden zweiten geraden Linie gebildet ist, und wobei der zweite Winkel durch die erste gerade Linie und eine Verlängerung einer das eine und das andere Ende eines nächsten Spline-Kurvensegmentes verbindenden dritten geraden Linie gebildet ist; und ein drittes Mittel zum Bewirken, dass ein Steuergerät die Spline-Kurvensegmente derart verbindet, dass: eine Richtung eines der Tangentenvektoren an dem einen Ende des einen Spline-Kurvensegmentes im wesentlichen gleich der eines der Tangentenvektoren an dem gleichen Ende des vorhergehenden Spline-Kurvensegmentes ist; und eine Richtung des anderen der Tangentenvektoren an dem anderen Ende des einen Spline-Kurvensegmentes im wesentlichen gleich der eines der Tangentenvektoren an dem gleichen Ende des nächsten Spline-Kurvensegmentes ist.
Programmprodukt nach Anspruch 3, wobei die Tangentenvektoren an dem einen Ende und an dem anderen Ende des einen Spline-Kurvensegmentes derart gerichtet sind, dass sie den ersten Winkel und den zweiten Winkel jeweils halbieren.
Verfahren des Bestimmens einer Spline-Kurve als eine Trajektorie eines vorbestimmten beweglichen Abschnittes eines Roboterkörpers, wobei das Verfahren umfasst: sequentielles Interpolieren von Spline-Kurvensegmenten jeweils in Abschnitten, die Intervallen zwischen Anweisungspunkten entsprechen; Bestimmen von Tangentenvektoren an einem und an dem anderen Ende eines Spline-Kurvensegmentes jedes der Abschnitte so, dass: eine Länge jedes der Tangentenvektoren proportional zu der einer das eine und das andere Ende des einen Spline-Kurvensegmentes verbindenden ersten geraden Linie ist; und die Tangentenvektoren an dem einen Ende und an dem anderen Ende des einen Spline-Kurvensegmentes derart gerichtet sind, dass sie einen ersten Winkel und einen zweiten Winkel jeweils in einem vorbestimmten Verhältnis teilen, wobei der erste Winkel durch die erste gerade Linie und eine Verlängerung einer das eine und das andere Ende eines vorhergehenden Spline-Kurvensegmentes verbindenden zweiten geraden Linie gebildet wird, und wobei der zweite Winkel durch die erste gerade Linie und eine Verlängerung einer das eine und das andere Ende eines nächsten Spline-Kurvensegmentes verbindenden dritten geraden Linie gebildet wird; und Verbinden der Spline-Kurvensegmente so, dass: eine Richtung des einen der Tangentenvektoren an dem einen Ende des einen Spline-Kurvensegmentes im wesentlichen gleich der eines der Tangentenvektoren an dem gleichen Ende des vorhergehenden Spline-Kurvensegmentes ist; und eine Richtung des anderen der Tangentenvektoren an dem anderen Ende des einen Spline-Kurvensegmentes im wesentlichen die gleiche ist, wie die eines der Tangentenvektoren an dem gleichen Ende des nächsten Spline-Kurvensegmentes.
Verfahren nach Anspruch 5, wobei die Tangentenvektoren an einem und an dem anderen Ende des einen Spline-Kurvensegmentes derart gerichtet sind, dass sie den ersten Winkel und den zweiten Winkel jeweils halbieren.