-
Die vorliegende Erfindung bezieht sich auf ein Verfahren
zum automatischen Einstellen des Arbeitspunkts eines
Endwirkorgans, d. h. einer Werkzeugspitze (TCP), in einem
Industrieroboter.
-
Beim Montieren eines Werkzeugs auf einer
Werkzeugmontageoberfläche eines Manipulatorteils, d. h. einer Frontplatte,
eines Roboters muß die Koordinatenposition des Punkts einer
Werkzeugspitze in einen Frontplatten-Koordinatensystem,
dessen Nullpunkt in dem Mittelpunkt der Frontplatte liegt,
eingestellt werden. Wenn ein Werkzeug benutzt wird, dessen
Spitzenpunktposition unbekannt ist, wird daher eine
derartige Einstellung herkömmlicherweise durch Eestiiniaen der
Koordinatenposition des Werkzeugspitzenpunkts auf der
Grundlage der Konstruktionszeichnung des Werkzeugs und manuelles
Eingeben numerischer Werte, die kennzeichnend für die
Position sind, in eine Steuereinrichtung des Roboters bewirkt.
-
Oftmals stehen jedoch die Konstruktionszeichnungen des
Werkzeugs nicht zur Verfügung. In einem solchen Fall wird
die Koordinatenposition des Werkzeugs herkömmlicherweise in
der zuvor genannten Art eingestellt, nachdem die Position
aus vorliegenden Ergebnissen von Messungen verschiedener
Teile des Werkzeugs, die mittels eines Meßwerkzeugs
gewonnen sind, bestimmt ist. Das Vermessen des Werkzeugs ist
jedoch mühsam, und es sind wesentliche Fehlermöglichkeiten
gegeben.
-
Dementsprechend liegt der vorliegenden Erfindung die
Aufgabe zugrunde, ein Verfahren zum automatischen Einstellen
eines Werkzeugspitzenpunkts zu schaffen, bei dem die
Position des Werkzeugspitzenpunkts in einem
Frontplatten-Koordinatensystem
eines Roboters leicht und genau eingestellt
werden kann, ohne dar dazu Konstruktionszeichnungen oder
eine Werkzeugvermessung mittels eines Meßwerkzeugs
erforderlich wären.
-
Die Druckschrift JP-A-61-133409 offenbart ein automatisches
Korrektursystem zum Korrigieren von Einstellungsfehlern
betreffend Roboterachsen und einen Werkzeugversatz. Die
Spitze des Werkzeugs wird an einem festgelegten vorbestimmten
Punkt bei einer Vielzahl unterschiedlicher Stellungen des
Roboters angeordnet.
-
Gemäß der vorliegenden Erfindung ist ein Verfahren
vorgesehen zum automatischen Einstellen des Punkts einer
Werkzeugspitze eines Werkzeugs, das auf einer Frontplatte eines
Arms eines Roboters montiert ist, wobei der Punkt der
Werkzeugspitze in einem Frontplatten-Koordinatensystem
einzustellen ist, das für die Frontplatte definiert ist, welches
Verfahren durch die folgende Sequenz von Schritten
gekennzeichnet ist: (a) Veranlassen des Roboters, zumindest vier
verschiedene Stellungen einzunehmen, wobei sich in allen
dieser Stellungen der Punkt der Spitze des Werkzeugs an
demselben Punkt innerhalb eines Raums befindet, in dem der
Roboter aufgestellt ist, und Speichern der betreffenden
Koordinatenpositionen eines Mittelpunkts der Frontplatte des
Roboters in einem Basis-Koordinatensystem, das in dem Raum
definiert ist, jedesmal dann, wenn der Roboter eine der
verschiedenen Stellungen einnimmt, (b) Berechnen der in dem
Basis-Koordinatensystem befindlichen Position des Zentrums
der Krümmung einer sphärischen Oberfläche, die durch die
betreffenden Koordinatenpositionen des Mittelpunkts der
Frontplatte, welcher in Schritt (a) gespeichert ist, oder
nahe diesen verläuft, und (c) Bestimmen und Einstellen der
Koordinatenposition des Punkts der Werkzeugspitze in dem
Frontplatten-Koordinatensystem, das für die Frontplatte
definiert ist, auf der Grundlage von der in dem
Basis-Koordinatensystem befindlichen Position des Zentrums der
Krümmung
der sphärischen Oberfläche, welche Position auf diese
Weise berechnet ist, von zumindest einer der betreffenden
Koordinatenpositionen in dem Basis-Koordinatensystem des
Frontplatten-Mittelpunkts und von Werkzeuglage-Vektoren der
Lage des Werkzeugs in dem Basis-Koordinatensystem
-
Gemäß der vorliegenden Erfindung, wie sie zuvor beschrieben
ist, wird der Werkzeugspitzenpunkt automatisch lediglich
durch Eingeben von vier oder mehr Punkten zur Instruktion
eingestellt, während die Stellung des Roboters in einem
Zustand derart verändert wird, daß der Werkzeugspitzenpunkt
in dem bestimmten Punkt innerhalb des
Roboter-Aufstellungsraums angeordnet wird, so daß der Werkzeugspitzenpunkt
leicht mit hoher Genauigkeit eingestellt werden kann, ohne
das dazu eine manuelle Eingabeoperation auf der Grundlage
von Konstruktionszeichnungen oder vorliegender Ergebnisse
von Vermessungen des Werkzeugs, die mittels eines
Meßwerkzeugs gewonnen sind, erforderlich wären.
-
Fig. 1 zeigt eine Darstellung zum Verdeutlichen des
Prinzips der vorliegenden Erfindung.
-
Fig. 2 zeigt eine Darstellung zum Verdeutlichen der
Beziehung zwischen einem Basis-Koordinatensystem und
einem Frontplatten-Koordinatensystem.
-
Fig. 3 zeigt ein Blockschaltbild, das einen Roboter
darstellt, auf welchen ein Verfahren zum automatischen
Einstellen eines Werkzeugspitzenpunkts gemäs einem
Ausführungsbeispiel der vorliegenden Erfindung
angewendet wird, und den wesentlichen Teil einer
Steuerschaltung zum Steuern des Roboters.
-
Fig. 4 zeigt ein Arbeitsflußdiagramm des
Ausführungsbeispiels.
-
Fig. 5 zeigt eine Darstellung zum Erläutern eines
Verfahrens
zum näherungsweise Bestimmen einer Kugel in
Übereinstimmung mit verschiedenen Instruktionen.
-
Fig. 6 zeigt eine Darstellung zum Erläutern eines
Verfahrens zum Gewinnen des Vektors des Mittelpunkts der
Kugel in dem Frontplatten-Koordinatensystem in jedem
Instruktionspunkt.
-
Im folgenden wird das Prinzip der vorliegenden Erfindung
anhand von Fig. 1 u. Fig. 2 beschrieben.
-
In Fig. 1 bezeichnen die Bezugszeichen 1, 2 u. 3 einen Arm
eines Roboters, ein Werkzeug, das an einer Frontplatte des
Arms 1 angebracht ist, bzw. den Mittelpunkt der
Frontplatte.
-
Um die Koordinatenposition des Spitzenpunkts des Werkzeugs
2 zu bestimmen, wird in einen
Frontplatten-Koordinatensystem, das für die Frontplatte 3 eingerichtet ist, der
Roboter veranlaßt, in Aufeinanderfolge vier Stellung
einzunehmen, wobei der Werkzeugspitzenpunkt in einem Punkt 4 in
dem Roboteraufstellungsraum angeordnet wird. Wenn der
Roboter jede Stellung einnimmt, ist die Koordiantenposition
jedes von Frontplatten-Mittelpunkten 3 in einem
Basis-Koordinatensystem, das in dem zuvor genannten Raum eingerichtet
ist, zur Instruktion gegeben.
-
Es sei hier vorausgesetzt, daß eine Koordinatenposition P1
des Frontplatten-Mittelpunkts zu der Zeit einer ersten
Instruktion durch (x1, y1, z1), eine Koordinatenposition P2
des Frontplatten-Mittelpunkts zu der Zeit einer zweiten
Instruktion durch (x2, y2, z2) und in gleicher Weise ein
dritter und ein vierter Instruktionspunkt P3 u. P4 durch
(x3, y3, z3) bzw. (x4, y4, z4) gegeben sind. Da der
Werkzeugspitzenpunkt des Werkzeugs 2 stets in dem einen Punkt 4
angeordnet wird, liegen diese Instruktionspunkte P1 bis P4
auf einer Kugeloberfläche (im folgenden als Kugel
bezeichnet),
deren Mittelpunkt in dem einen Punkt 4 liegt.
Daraufhin gelten, wenn die Koordinatenposition des Punkts 4 in
dem Basis-Koordinatensystem und der Radius der Kugel (oder
die Werkzeuglänge) (a, b, c) bzw. r sind, die folgenden
Gleichungen für die zuvor genannten innstruktionspunkte P1
bis P4:
-
(x1 - a)² + (y1 - b)² + (z1 - c)² = r² ... (1)
-
(x2 - a)² + (y2 - b)² + (z2 - c)² = r² ... (2)
-
(x3 - a)² + (y3 - b)² + (z3 - c)² = r¹ ... (3)
-
(x4 - a)² + (y4 - b)² + (z4 - c)² = r² ... (4)
-
In den Gl. (1) bis (4) sind x1 bis x4, y1 bis y4 u. z1 bis
z4 bekannte Werte, die zu der Zeit der einzelnen
Instruktionen gewonnen sind, und die Unbekannten sind a, b, c u.
r. Demzufolge können der Mittelpunkt (a, b, c) der Krümmung
der Kugel (im folgenden als der Mittelpunkt der Kugel
bezeichnet) und der Radius r der Kugel durch Lösen der vier
Gleichungen (1) bis (4) des Gleichungssystems gewonnen
werden.
-
Es sei nun darüber hinaus vorausgesetzt, daß Vektoren, die
von dem Nullpunkt 0 des Basis-Koordinatensystems 6 aus auf
den Punkt 4 und von dem Mittelpunkt 3 der Frontplatte aus
auf den Punkt 4 gerichtet sind, durch die Bezugszeichen 7
bzw. 8 bezeichnet sind, die Bezugszeichen 10, 11 u. 12
Werkzeuglagenvektoren bezeichnen, die Komponenten der
Vektoren 8 u. 7 durch (XH, YH, ZH) bzw. (XB, YB, ZB) gegeben
sind, die Komponenten der Lagenvektoren 10, 11 bzw. 12
durch (lX, lY, lZ), (mX, mY, mZ) bzw. (nX, nY, nZ) gegeben
sind und der Mittelpunkt der Frontplatte durch (OX, OY, OZ)
gegeben ist. Daraufhin gelten folgende Gleichungen:
-
Aus Gl. (5) wird gewonnen:
-
Da (XB, YB, ZB) des Vektors 7 der Wert (a, b, c) ist, der
durch Lösen der Gleichungen des Gleichungssystems (1) bis
(4) gewonnen ist, kann die vorstehend angegebene Gleichung
wie folgt umgestellt werden:
-
In Gl. (6) sind die Lagenvektoren 10 (lX, lY, lZ), 11 (mX,
mY, mZ) u. 12 (nX, nY, nZ) Werte, die aus dem Werten von
verschiedenen Gelenkwinkeln des Roboters gewonnen sind,
welche durch ein herkömmliches Verfahren erfaßbar sind, und
der Mittelpunkt (OX, OY, OZ) der Frontplatte ist ein
Instruktionspunkt. Demzufolge wird durch Einsetzen dieser
Werte in Gl. (6) die Komponente (XH, YH, ZH) des Vektors 8,
d. h. die Koordinatenposition (XH, YH, ZH) des
Werkzeugspitzenpunkts in dem Frontplatten-Koordinatensystem
gewonnen, und diese position kann als der Werkzeugspitzenpunkt
eingestellt werden.
-
Im folgenden wird anhand von Fig. 3 bis Fig. 6 ein
Verfahren zum automatischen Einstellen eines
Werkzeugspitzenpunkts gemäß einem Ausführungsbeispiel der vorliegenden
Erfindung beschrieben.
-
Fig. 3 zeigt einen Roboter, auf den das Verfahren gemäß der
Erfindung angewendet wird, sowie periphere Elemente. Der
Roboter ist mit einem Steuerabschnitt 51 versehen, der eine
zentrale Verarbeitungseinheit (im folgenden als CPU
bezeichnet) enthält. Der Steuerabschnitt 51 ist mit einem
Speicher 52, der aus einem ROM gebildet ist, mit einem
Speicher 53, der aus einem RAM gebildet ist, einem
Befehlsbetätigungsfeld 54, einem Arbeitsfeld 55 mit einer
Katodenstrahlröhren-Anzeigeeinrichtung und einem Bandlesegerät 56
verbunden.
-
Der Speicher 52 ist mit verschiedenen Steuerprogrammen
geladen, die mittels des Steuerabschnitts abzuarbeiten sind.
Der Speicher 53 wird mit Befehlsdaten, die über das
Befehlsbetätigungsfeld 54, das Arbeitsfeld 55, das
Bandlesegerät 56 usw. eingegeben werden, mit Daten, die über das
Arbeitsfeld 55 eingegeben werden, und Ergebnissen und Daten
einer Berechnung, die mittels des Steuerabschnitts 51
ausgeführt wird, geladen. Das Befehlsbetätigungsfeld 54 hat
eine numerische Anzeigeeinrichtung, Lampen und
Bedienungstasten, die für den Betrieb des Roboters erforderlich sind.
Das Arbeitsfeld oder die Dateneingabeeinrichtung (im
folgenden als CRT/MDT bezeichnet) 55 mit der
Katodenstrahlröhren-Anzeigeeinrichtung enthält sowohl verschiedene Tasten,
wie 10er-Block-Tasten und Funktionstasten, als auch eine
Ahzeigeeinheit, die eine Katodenstrahlröhre CRT enthält.
Eine Bedienungsperson kann verschiedene Daten über die
CRT/MDI 55 eingeben. Das Bezugszeichen 60 bezeichnet eine
Achsensteuereinrichtung, die einen Interpolator enthält und
für die Achsensteuerung einer Vielzahl von Achsen bestimmt
ist, das Bezugszeichen 59 bezeichnet eine Servoschaltung
zum Regeln einer Antriebsquelle des Roboters 58, und das
Bezugszeichen 64 bezeichnet eine Eingabe/Ausgabeschaltung,
die Eingabe/Ausgabeoperationen für Signale durchführt, die
mittels einer Relaiseinheit 63 zu oder von einem Objekt 61
der Operation geliefert werden. Das Bezugszeichen 57
bezeichnet eine Eingabe/Ausgabeschaltung, durch die
verschiedene Erfassungssignale von dem Roboter an den Eingang des
Steuerabschnitts 51 gelegt werden. Das Bezugszeichen 62
bezeichnet eine Schnittstelle für eine
Schnittstellenverbindung mit dem Objekt der Operation, und das Bezugszeichen
66 bezeichnet eine Busleitung. Da diese Einrichtungen
mittels eines herkömmmlichen Verfahrens aufgebaut werden
können, kann eine ins einzelne gehende Beschreibung entfallen.
-
Im folgenden wird anhand Fig. 4 das Verfahren gemäß der
vorliegenden Erfindung beschrieben.
-
Wenn die CRT/MDI 55 erstmals betätigt wird, um eine
Betriebsart für das automatische TCP-Einstellverfahren
(automatische Werkzeugspitzenpunkt-Einstellverfahren)
einzustellen, setzt die CPU in dem Steuerabschnitt 51 einen
Index K auf "0" (Schritt S1) und entscheidet dann, ob eine
Eingabe in Reaktion auf die Betätigung einer
Befehlsendetaste oder einer Befehlstaste des Befehlsbetätigungsfelds
54 erfolgt ist oder nicht (Schritte S2 u. S3) . Wenn die
Bedienungsperson die Befehlstaste nach dem Anordnen der
Werkzeugspitze in dem Raum in dem einen Punkt 4 (s. Fig. 1)
niederdrückt, veranlaßt die CPU den Speicher 53, die
Instruktionsposition Pi (= P1), d. h. die Koordinatenposition
Pi (xi, yi, zi) des Mittelpunkts der Frontplatte, und die
dann geltenden Lagenvektoren (lX, lY, lZ), (mX, mY, mZ) u.
(nX, nY, nZ) zu speichern (Schritt S4) . Die Unterangabe i
bezeichnet den Rang des Befehls und ist durch i = K + 1
gegeben. Nachdem der Index K dann um "1" erhöht ist (Schritt
S5), kehrt das Programm zu Schritt S2 zurück und wartet auf
die Eingabe des nächsten Instruktionsbefehls (Schritt S3).
Danach veranlaßt die CPU jedesmal dann, wenn die
Bedienungsperson einen Instruktionsbefehl nach einer Änderung
der Stellung des Roboters 58 in einem Zustand eingibt, in
dem der Werkzeugspitzenpunkt in dem vorbestimmten Punkt 4
angeordnet ist, den Speicher, die Instruktionsposition Pi
(xi, yi, zi) zu speichern. Diese Prozesse werden
wiederholt, um den Roboter über zumindest vier Punkte zu
instruieren.
-
Theoretisch kann der Werkzeugspitzenpunkt in
zufriedenstellender Weise durch Instruieren des Roboters über die
Koordinatenpositionen, wie sie vorstehend anhand von Fig. 1
erläutert sind, eingestellt werden. In dem vorliegenden
Ausführungsbeispiel sind indessen vier oder mehr Punkte zur
Instruktion gegeben, so daß die Kugel unter der Annahme
eines Falls, in dem der Werkzeugspitzenpunkt nicht genau in
dem vorbestimmten Punkt 4 angeordnet werden kann,
näherungsweise auf der Grundlage dieser vier oder mehr
Instruktionspunkte, wie untenstehend erläutert, gewonnen wird.
-
Daraufhin setzt sich, wenn die Bedienungsperson eine
Instruktion über die vier oder mehr Punkte und dann einen
Instruktionsendebefehl eingibt, das Program gemäß Fig. 4 zu
Schritt S6 fort, und die CPU bestimmt näherungsweise die
Kugel 5, wie dies in Fig. 5 gezeigt ist, auf der Grundlage
der einzelnen Positionen P1 (x1, y1, z1) bis Pk (xk, yk,
zk), die zur Instruktion gegeben sind, und gewinnt dann den
Mittelpunkt (a, b, c) der Kugel 5. Der Index K, welcher in
Schritt S5 mit jeder Instruktion um "1" erhöht wird, gibt
die Anzahl von Instruktionspunkten an.
-
Bei den vorliegenden Ausführungsbeispiei wird die Kugel
näherungsweise mittels einer Anzahl K von
Instruktionspunkten P1 bis Pk bestimmt, und es wird das Verfahren "Kleinste
Quadrate" benutzt, um den Mittelpunkt (a, b, c) und den
Radius r der Kugel zu gewinnen (Schritt S6)
-
Im einzelnen ist eine Gleichung, die repräsentativ für die
Kugel ist, welche den Radius r in der Position (a, b, c)
hat, gegeben durch
-
(x - a)² + (y - b)² + (z - c)² = r² ... (7)
-
Falls eine Abweichung bei jeder Instruktionsposition Pi
(xi, yi, zi) vorliegt und die quadratische Summe der
Abweichung ε1 bzw. E ist, gelten folgende Beziehungen:
-
εi = (xi - a)² + (yi - b)² + (zi - c)² - r²,
-
εi² = { (xi - a)² + (yi - b)² + (zi - c)² + r²) }²
-
= (xi - a)&sup4; + (yi - b)&sup4; + (zi - c)&sup4; + r&sup4;
-
+ 2(xi - a)²(yi - b)² + 2(yi - b)²(zi - c)²
-
+ 2 (zi - c)²(xi - a)² - 2r² (xi - a)²
-
- 2r²(yi - b)² - 2r²(zi - c)²,
-
δE/δr = 4kr³ - 4rΣ(yi - b)² - 4rΣ(zi - c)²
-
= 4r{(kr² - Σ(xi - a)² - Σ(yi - b)² - Σ(zi - c)²}.
-
Wenn δE/δr T 0 gegeben ist, gilt, um den Wert r
entsprechend dem Minimalwert der quaratischen Summe E der
Abweichung zu gewinnen, die folgende Gleichung:
-
r = 0
-
oder
-
kr² - Σ(xi - a)² - Σ(yi - b)² - Σ(zi - c)² = 0
-
Indessen gilt für r:
-
r ≠ 0
-
so daß gewonnen wird:
-
r² = {Σ(xi - a)² + Σ(yi - b)² + Σ(zi - c)²}/k ... (8)
-
Um den Punkt (a, b, c) zugewinnen, der dem Minmialwert der
quaratischen Summe der Abweichung entspricht, wird darüber
hinaus eine Bedingung derart, das das partielle
Differential der Summe E jeder der Variablen a, b u. c unendlich
klein ist, berechnet. Zunächst wird das partielle
Differential
der Summe E für die Variable a wie folgt gewonnen:
-
δE/δa = -4Σ(xi - a)³ - 4S(xi -a) (yi - b)²
-
-4Σ(xi - a) (zi - c)&sub2; + 4r²Σ(xi - a).
-
Hierbei wird, wenn δE/δr T 0 gegeben ist, gewonnen:
-
Σ(xi - a)³+Σ(xi - a) (yi - b)² + (zi - c)² - r² = 0
-
Σxi³ - 3Σxi²a + 3Σxia² - ka³
-
+ Σ(yi - b)² + (zi - c)² - r²)xi
-
- aΣ(yi - b)² + (zi - c)² - r²} = 0.
-
Aus Gl. (7) wird gewonnen:
-
Σxi³ - 3Σxi²a + 3Σxia² - ka³
-
+ Σyi²xi - 2Σyixib + Σxib²
-
+ Σzi²xi - 2Σzixic + Σxic²
-
- Σr²xi - aΣ(xi - a)² = 0.
-
Aus Gl. (8) wird gewonnen:
-
Σxi³ - 3Σxi²a + 3Σxia² - ka³
-
+ Σyi² - 2Σyixib + Σxib²
-
+ Σzi²xi - 2Σzixic + Σxic²
-
(wobei j = 1, 2 . . . k) ist.
-
Durch Entfernen der Klammern der vorstehenden Gleichung
wird gewonnen:
-
Σxi³ - 3Σxi²a + 3Σxia² - ka³
-
+ Σyi²xi - 2Σyixib + Σxib²
-
+ Σzi²xi - 2Σzixic + Σxic²
-
- ΣxiΣxj²/k + 2aΣxiΣxj/k - a²Σxi
-
- ΣxiΣyj²/k + 2bΣxiΣyj/k - b²Σxi
-
- ΣxiΣzj²/k + 2cΣxiΣzj/k - c²Σxi
-
+ aΣxi² - 2a² Σxi + ka³ = 0.
-
Durch Umstellen der vorstehenden Gleichung wird gewonnen:
-
Σxi³ - 2aΣxi² + Σxiyi² - 2bΣxiyi
-
+ Σxizi² + 2cΣxizi
-
- ΣxiΣxj²/k - ΣxiΣyj²/k - ΣxiΣzj²/k
-
+ 2aΣxiΣxj/k + 2bΣxiΣyj/k + 2cΣxiΣzj/k = 0 ... (9)
-
Wenn die Koeffizienten der Variablen a, b u. c von 01. (9)
A1, A2 bzw. A3 sind und wenn der konstante Term A4 ist,
wird gewonnen:
-
A1 a + A2 b + A3 c + A4 = 0 ... (10)
-
Hierbei sind die Koeffizienten A1 bis A3 und der konstante
Term A4 gegeben durch
-
A1 = 2ΣxiΣxj/k - 2Σxi²,
-
A2 = 2ΣxiΣyj/k - 2Σxiyi,
-
A3 = 2ΣxiΣzj/k - 2Σxizi,
-
A4 = Σxi³ + Σxjyi² + Σxjzi²
-
- ΣxiΣxj² + Σyj² + Σzj²/k.
-
Desgleichen wird die quadratische Summe E der Abweichung
einer partiellen Differentiation für die Variable b(&DE/δb)
unterzogen. Wenn &/DE/δb = 0 gegeben ist, sind die
Gleichung und die Koeffizienten B1, B2 u. B3 der Variablen a, b
u. c und ein konstanter Term B4 wie folgt gegeben:
-
B1 a
+ B2 b + B3 c + B4 = 0, ... (11)
-
B1 = 2ΣyiΣxj/k - 2Σxiyi,
-
B2 = 2ΣyiΣyj/k - 2Σyi²,
-
B3 = 2ΣyiΣzj/k - 2Σyizi,
-
B4 = Σyizi² + Σyi³ + Σyixi²
-
- Σyj²(Sxj² + Σyj² + Σzj²)/k.
-
Die quadratische Sume E der Abweichung wird einer
partiellen Differentiation für die Variable c unterzogen. Wenn
δE/δb = 0 gegeben ist, sind die Gleichung und die
Koeffizienten C1, C2 u. C3 der Variablen a, b u. c sowie ein
konstanter Term C4 wie folgt gegeben:
-
C1 a + C2 b + C3 c + C4 = 0 ... (12)
-
C1 = 2ΣziΣxj/k - 2Σxizi,
-
C2 = 2ΣziΣxj/k - 2Σyizi,
-
C3 = 2ΣziΣzj/k - 2Σzi²,
-
C4 = Σxi²zi + Σyi²zi + Σzi³
-
- Σzi(Σxj² + Σyj² + Σzj²)/k.
-
Σ repräsentiert hierbei
-
Σxi u. Σxj repräsentieren beispielweise
-
Da die Werte xi, yi u. zi in Reaktion auf die Instruktion
des Schritts S3 gewonnen und gespeichert werden, werden die
Koeffizienten und konstanten Terme A1 bis A4, B1 bis B4 u.
C1 bis C4 aus diesen gespeicherten Werten gewonnen. Als
Folge davon können die Unbekannten a, b, c aus den ternären
linearen Gleichungen des Gleichungssystems (10), (11), (12)
gewonnen werden (Schritt S6).
-
Daran anschließend wird Gl. (8) aufgelöst, um den Radius r
der Kugel 5 in Übereinstimmung mit dem Mittelpunkt (a, b,
c), der in Schritt S6 gewonnen ist, und den einzelnen
gespeicherten Positionen (x1, y1, z1) zu gewinnen (Schritt
S7).
-
In anderen Worten ausgedrückt heißt dies, daß nach
-
r = {(Σ(xi - a)² + Σ(yi - b)² + Σ(zi - c)²}/k]
-
aufgelöst wird, um den Radius r zu gewinnen.
-
Auf der Grundlage des Mittelpunkts (a, b, c) der Kugel und
es Radius r der Kugel, welcher auf diese Weise gewonnen
ist, wird der Vektor (XH, YH, ZH), d. h. die
Koordinatenposition (XH, YH, ZH) des Werkzeugspitzenpunkts (Mittelpunkt
der Kugel) in dem Fromtplatten-Koordinatensystem mit dessen
Nullpunkt in dem Mittelpunkt der Frontplatte gewonnen. Mehr
im einzelnen wird der Vektor (XHi, YHi, ZHi) für jede
Instruktionsposition Pi in den Prozessen der folgenden
Schritte S8 bis S16 gewonnen, und die gewonnenen Werte
werden gemittelt, um den zuvor genannten Vektor (XH, YH, ZH)
zu gewinnen.
-
Zunächst wird der Index i in Schritt S8 auf "1" gesetzt,
und es wird eine Transformationsmatrix Gi bei der
Instruktionsposition Pi (= P1) auf der Grundlage der Lagenvektoren
(lX, lY, lZ), (mX, mY, mZ) u. (nX, nY, nZ) bei der
Instruktionsposition Pi und dem Mittelpunkt oX, oY, oZ) der
Frontplatte eingerichtet. Da der Mittelpunkt der Frontplatte in
der Instruktionsposition liegt, gilt:
-
(oX, oY, oZ) = (xi, yi, zi)
-
Da der Lagenvektor von dem Wert des Gelenkwinkels des
Roboters abhängt, wird darüber hinaus die Transformationsmatrix
Gi wie folgt gewonnen:
-
Dann wird die Berechnung von Gl. (6) auf der Grundlage der
Transformationsmatrix Gi, die auf diese Weise gewonnen ist,
und des Mittelpunkts (a, b, c) bewirkt, und es wird ein
Vektor (Xoi, Yoi, Zoi), der von dem Mittelpunkt 3 der
Frontplatte bei der Instruktionsposition aus Pi auf den
Mittelpunkt (a, b, c) gerichtet ist, gewonnen (Schritt
S10) . Auf der Grundlage des Vektors (Xoi, Yoi, Zoi), der
auf diese Weise gewonnen ist, wird ein Radius roi gemäß der
folgenden Gleichung gewonnen (Schritt S11):
-
roi = [Xoi² + Yoi² + Zoi²]
-
Auf diese Weise werden der Vektor (Xoi, Yoi, Zoi) und die
Größe (der Radius) roi gewonnen, wie dies in Fig. 6 gezeigt
ist.
-
Indessen ist der zu gewinnende Vektor (XHi, YHi, ZHi) ein
Vektor (dessen Größe gleich dem Radius r ist), der von
einem Punkt 3' auf der näherungsweise gewonnenen Kugel 5 aus
auf den Punkt 4 gerichtet ist.
-
Daraufhin gilt die folgende Beziehungsgleichung (13)
zwischen dem Vektor (Xoi, Yoi, Zoi) und dem Vektor (XHi, YHi,
ZHi)
-
r/roi = XHi/Xoi = YHi/Yoi = ZHi/Zoi ... (13)
-
Aus Gl. (13) wird gewonnen:
-
XHI = Xoi.r/roi, ... (14)
-
YHi = Yoi.r/roi, ... (15)
-
ZHi = Zoi.r/roi ... (16)
-
Daraufhin werden die Gl. (14) bis (16) in Übereinstimmung
mit (Xoi, Yoi, Zoi), gewonnen in Schritt S10, und roi,
gewonnen in Schritt S11, berechnet, um den Vektor (XHi,
YHi, ZHi) zu gewinnen (Schritt S12).
-
Dann werden die Vektorwerte XH, YH, ZM, die in Schritt S12
gewonnen sind, zu den Werten der Register R(XH), R(YH) bzw.
R(ZH) addiert und gespeichert (Schritt S13).
-
Die Register (R(XH) bis R(ZH) werden vorab initialisiert
und auf "0" zurückgesetzt, wenn das automatische
Werkzeugspitzen-Einstellverfahren eingerichtet wird.
-
Anschließend wird entschieden, ob der Index i gleich dem
Wert des Index K ist, welcher kennzeichnend für eine
vorbestimmte Häufigkeit der Instruktion ist (Schritt S14) . Falls
die zwei Werte nicht gleich sind, wird der Index i um "1"
erhöht (Schritt S15, und die Prozesse des Schritts S9 und
der nachfolgenden Schritte werden wiederholt. Als Ergebnis
wird ein Vektor, der von dem Frontplatten-Mittelpunkt 3 bei
jeder Instruktonsposition aus auf den Mittelpunkt (a, b, c)
der Kugel 5, d. h. die Koordinatenposition des
Werkzeugspitzenpunkts in dem Frontplatten-Koordinatensystem
gerichtet ist, zu jedem der Register (R(XH), R(YH) u. R(ZH)
addiert. Wenn der Index i die Instruktionshäufigkeitszahl K
er-reicht, werden die betreffenden Werte der Register
R(XH), R(YH) u. R(ZH) durch die Instruktionshäufigkeitszahl
K dividiert. Auf diese Weise wird der zuvor genannte
gemittelte Vektor (XH, YH, ZH), d. h. die Koordinatenposition
des Werkzeugspitzenpunkts in dem
Frontplatten-Koordinatensystem, gewonnen (Schritt S16)
-
Wenn der Wert (XH, YH, ZH) als die
Werkzeugspitzen-Koordinatenposition in dem Speicher 53 gespeichert ist, wird dann
der Werkzeugspitzenpunkt automatisch eingestellt.
-
In dem zuvor beschriebenen Ausführungsbeispiel werden die
Vektoren (XHI, YHi, ZHi) bei den einzelnen
Instruktionspunkten gewonnen, und der Vektor (XH, YH, ZH) wird durch
den Mittelwert dieser Werte betimmt. Falls die
erforderliche Genauigkeit indessen nicht sehr hoch ist, kann der
Vektor, d. h. der Werkzeugspitzenpunkt in dem
Koordinatensystem mit dessen Nullpunkt in dem Mittelpunkt der
Frontplatte durch Bewirken der Berechnung von Gl. (6) auf der
Grundlage des Mittelpunkts (a, b, c) der Kugel, welcher in
Schritt S6 gewonnen ist, und der Koordinatenposition jedes
der Instruktionspunkte gewonnen werden.