PatentDe  


Dokumentenidentifikation DE19810341C2 12.10.2000
Titel Verfahren zur automatischen Kollisionsvermeidung eines Manipulators in einem durch Hindernisse beschränkten Arbeitsraum
Anmelder Deutsches Zentrum für Luft- und Raumfahrt eV, 53175 Bonn, DE
Erfinder Schlemmer, Maximilian, Dr., 82402 Seeshaupt, DE;
Steinmetz, Michael, 82234 Weßling, DE;
Schedl, Manfred, 85276 Pfaffenhofen, DE;
Finsterwalter, Reinhard, Prof., 82178 Puchheim, DE
Vertreter von Kirschbaum, A., Dipl.-Ing., Pat.-Anw., 82110 Germering
DE-Anmeldedatum 10.03.1998
DE-Aktenzeichen 19810341
Offenlegungstag 16.09.1999
Veröffentlichungstag der Patenterteilung 12.10.2000
Veröffentlichungstag im Patentblatt 12.10.2000
IPC-Hauptklasse B25J 9/18

Beschreibung[de]

Die Erfindung bezieht sich auf ein Verfahren zur automatischen Kollisionsvermeidung eines Manipulators in einem durch Hindernisse beschränkten Arbeitsraum auf der Basis eines geometrischen Weltmodells des Arbeitsbereichs und des Manipulators und von mit Hilfe einer Handsteuerkugel (Spacemouse) oder dergleichen von einem Programmierer kommandierten Endeffektor-Zielverschiebungen Δxd in Verbindung mit der Berechnung von Gelenkpositionswerten entsprechend einem Algorithmus inverser Kinematik, wobei von einem geometrischen Modell des Manipulators durch Kugeln, welche die einzelnen Teile, wie Arme und Gelenke, des Manipulators approximieren, sowie von einem geometrischen Modell der Oberflächen der Arbeitsraumhindernisse durch approximierende Punktegitter (Kollisionspunkte) ausgegangen wird.

Aus Siciliano, B., Sciavicco, L.: "Modelling and Control of Robot Manipulators", McGraw-Hill Companies (1996), Seiten 93 bis 97 und aus Vukobratovic, M., Kircanski, N.: "Kinematics and Trajectory Synthesis of Manipulation Robots", Springer- Verlag, Tokyo, 1986, Seiten 237 bis 257 sind Verfahren zur automatischen Kollisionsvermeidung eines Manipulators auf der Grundlage kommandierter Endeffektor-Zielvorgaben in Verbindung mit einer Berechnung entsprechend einem Algorithmus inverser Kinematik unter Benutzung der Jacobi-Matrix und auf Basis eines geometrischen Modells des Manipulators und der Arbeitsraumhindernisse bekannt.

Außerdem sind Verfahren mit Berechnung einer verallgemeinerten Inversen der Jacobi-Matrix bekannt, mit denen Kollisionsvermeidung durch Maximierung einer Abstandsfunktion als globales Kriterium im Nullraum der Jacobi-Matrix oder durch eine gewichtete, auf Basis der Ausdehnungen der Hindernisse im Arbeitsraum ausgerichtete Optimierung des zu berechnenden aktuellen Gelenkpositionsversatzes realisiert ist.

Von Nachteil ist hierbei, daß Bahnbeschränkungen durch physikalische Gelenkanschläge und maximale Gelenkgeschwindigkeiten nicht sicher eingehalten werden können, womit Bahnfehler des kommandierten Endeffektor-Ziels in einer Umgebung solcher Beschränkungen auftreten, sich ein instabiles Verhalten in singulären Roboterstellungen durch eine (verallgemeinerte) Invertierung der Jacobi-Matrix ergibt und ineffiziente Roboterbahnverläufe möglich sind, wenn widersprechende lokale und globale Kriterien auftreten.

In der Deutschen Zeitschrift "Robotersysteme" 7, (1991) S. 185 bis 192 ist ein Verfahren zur automatischen Kollisionsvermeidung eines Manipulators bzw. Roboters in einem durch Hindernisse beschränkten Arbeitsraum auf der Basis eines geometrischen Weltmodells des Arbeitsbereichs bzw. der Umwelt und des Roboters und von Endeffektor-Zielverschiebungen in Verbindung mit der Berechnung von Gelenkpositionswerten entsprechend einem Algorithmus offenbart, (vgl. Zusammenfassung, Seite 185; Kap. 1.3, Seite 188; Kap. 3.2.1, Seite 188; Kap. 3.2.1, Seite 189, letzter Absatz; Kap. 3.2.2 und Kap. 3.2.3), wobei die Arbeitsraumhindernisse durch Kollisionspunkte bzw. Distanzpunkte beschrieben werden (vgl. Kap. 3.2.1 Abs. 1).

Ferner wird nach dieser Druckschrift ausgehend von der Endeffektorzielverschiebung und dem aktuellen Istwert der Gelenkpositionen bzw. Winkelstellung unter Berücksichtigung einer zu minimierenden Gütefunktion bzw. Zielfunktion f, von Bahnbeschränkungen durch physikalische Gelenkanschläge des Roboters und der kinematischen Gleichung, welche durch die Jacobi-Matrix J der Kinematik repräsentiert wird, eine neue Gelenkposition bzw. Gelenkstellung berechnet (vgl. Kap. 3.2.2 und 3.2.3).

Da die Gelenkwinkel nach der Berechnung an die Robotersteuerung gesendet werden (vgl. Kap. 3.2.3, S. 190, letzter Absatz), werden durch die neue Gelenkposition die neuen Werte für die Gelenkregler vorgegeben. Die Parametrisierung der Güte- bzw. Zielfunktionen durch positive Wichtungswerte erfolgt durch die positiv definierte Matrix G in der zu minimierenden Funktion.

Einen Roboter durch eine Kugel geometrisch zu beschreiben bzw. zu modellieren, ist nicht nur aus der Zeitschrift "Robotersysteme" (vgl. Kap. 3.1 Bild 5) bekannt, sondern auch in US 5,347,459 (Spalte 5, letzter Absatz) ausgeführt.

AT 401 746 B (vgl. S. 4, Z.'n 15 bis 52) ist zu entnehmen, daß die zu minimierende Güte- bzw. Zielfunktion die Summe aus mehreren beliebig wählwaren Kriterien bzw. Beiträgen und Zusatzbedingungen bzw. -kriterien ist. Die Quadratsumme der Winkelgeschwindigkeit ist proportional zur Energie und stellt somit das Energiekriterium dar. Ein Referenzlagenkriterium ist aus dem Beitrag der Zielfunktion zu entnehmen, die den Abstand des Roboters von der gestreckten (oder irgendeiner anderen) Konfiguration bzw. Referenzlage beschreibt. Die Quadratsumme der Winkelbeschleunigung ist das Beschleunigungskriterium.

Bei dem in AT 401 746 B beschriebenen Verfahren sind wahlweise auch die maximalen Geschwindigkeiten und Beschleunigungen berücksichtigt. Ferner enthält ein in AT 401 746 B beschriebenes Kollisionskritierum eine(n) Abstandsfunktion bzw. -term und einen Parameter, der die Stärke der Abstoßung vom Hindernis (Potentialfeldmethode) beschreibt, d. h. eine Potentialgewichtung. Mittels eines sogenannten Zufall-Suchverfahrens wird die Güte- bzw. Zielfunktion für redundante Roboter minimiert und dadurch ein globales Minimum gefunden.

In DE 198 00 552 A2 ist ein Verfahren zur Kommandosteuerung eines Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel oder dgl. von einem Programmierer oder einer übergeordneten Aufgabe kommandierten Endeffektor-Zielverschiebungen in Verbindung mit einer Berechnung von Gelenkpositionswerten entsprechend einem Algorithmus inverser Kinematik beschrieben.

Ausgehend von einer kommandierten Endeffektor-Zielverchiebung und dem aktuellen Istwert der Gelenkpositionen des Manipulators wird unter Berücksichtigung einer zu minimierenden Gütefunktion f(q), die durch nichtnegative Gewichtungswerte αj, βj, γj parametriert ist und unter Berücksichtigung von Bahnbeschränkungen durch physikalische Gelenkanschläge, maximaler Gelenkgeschwindigkeit ≙max, maximaler Gelenkbeschleunigung ≙max in einer Umgebung physikalischer Gelenkanschläge und der kinematischen Gleichung, welche durch die Jakobi-Matrix J(q) repräsentiert ist, eine neue Gelenkposition qi+1 berechnet, die die neuen Werte für die Gelenkregler vorgibt, wobei die Gütefunktion f(q) die Summe aus Energiekriterium, Referenzlagenkriterium, Beschleunigungskriterium und einem Zusatzkriterium ist.

Dabei berechnet sich das Energiekriterium aus (q - qi)tdiag(αj)(q - qi), das Referenzlagenkriterium aus



(q - qref)tdiag(βj)(q - qref),



wobei die Größe qref ein vorgegebener Gelenkpositionswert ist, der derart festgelegt ist, daß die Folge der berechneten Gelenkpositionswerte qi nahe um diesen Referenzpositionswert verläuft, das Beschleunigungskriterium aus



(q - 2qi + qi-1)tdiag(γj)(q - 2qi + qi-1),



und das Zusatzkriterium aus -p, wobei der skalare Parameter p der kinematischen Gleichung p . Δxd = J(qi)(q - qi), und der Ungleichung 0 ≤ p ≤ 1 genügt, wobei p . 100 die prozentuale Realisierung der kommandierten Endeffektor-Zielverschiebung Δxd ist.

Ausgehend von einem Gelenkpositionswert qi als Startpunkt wird auf Basis der Gütefunktion ein zulässiger Optimierungsvektor bezüglich aller aktiven Nebenbedingungen bestimmt, die angeben, welche Bahnbeschränkungen erreicht sind und dieser wird skaliert entsprechend den inaktiven Nebenbedingungen, die angeben, welche Bahnbeschränkungen nicht erreicht sind.

Der skalierte Optimierungsvektor wird auf den im vorigen Iterationsschritt berechneten Gelenkpositionswert addiert und auf Basis der Gütefunktion und der in der neu berechneten Gelenkposition aktivierten Nebenbedingungen wird die Optimalität dieser Gelenkpositionswerte bewertet.

Der Erfindung liegt die Aufgabe zugrunde, ein Verfahren mit automatischer Kollisionsvermeidung zur Kommandosteuerung eines Manipulators zu schaffen, mit dem Bahnbeschränkungen durch physikalische Gelenkanschläge sowie maximal erlaubte Gelenkgeschwindigkeiten sicher eingehalten werden, mit dem eine minimale Belastung auf die Antriebsvorrichtung des Manipulators durch eine Optimierung des Beschleunigungsverhaltens der Gelenkachsen erreicht wird, mit dem eine maximale Beweglichkeit des Endeffektors durch Optimierung des Abstandes der Gelenkpositionen von den Anschlägen erreicht wird und mit dem Singularitäten in stabiler Weise um- bzw. durchfahren werden können.

Gemäß der Erfindung, die sich auf ein Verfahren zur automatischen Kollisionsvermeidung eines Manipulators der eingangs genannten Art bezieht, wird diese Aufgabe durch die Anwendung folgender Merkmale gelöst:

  • - der Manipulator wird durch Kugeln K(rk, xmk) mit Kugelmittelpunkten xmk∈ IR3 und Kugelradien rk > 0 geometrisch beschrieben und die Arbeitsraumhindernisse werden durch Kollisionspunkte xcolll ∈ IR3 geometrisch beschrieben,
  • - auf der Basis dieser Beschreibungen wird die glatte Maximum-Approximation aller Elementarabstände





    erklärt mit





    wobei der positive Parameter ρ dieser Gleichung die Güte der Maximumapproximation spezifiziert und von einem Benutzer vorgegeben wird,
  • - ausgehend von der kommandierten Endeffektor-Zielverschiebung und dem aktuellen Istwert qi der Gelenkpositionen des Manipulators wird
  • - unter Berücksichtigung einer zu minimierenden Gütefunktion f(q, p, porth), die durch nichtnegative Wichtungswerte αj, βj, γj, γd parametriert ist,
  • - und unter Berücksichtigung von Bahnbeschränkungen durch physikalische Gelenkanschläge qmin, qmax, maximaler Gelenkdrehzahlen ≙max und maximaler Gelenkbeschleunigungen ≙max in einer Umgebung physikalischer Gelenkanschläge und der kinematischen Gleichung, welche durch die Jacobi-Matrix J(q) der Kinematik repräsentiert ist,

    eine neue Gelenkposition qi+1 berechnet, welche die neuen Werte für die Gelenkregler vorgibt, wobei die Gütefunktion f(q, p, porth) die Summe aus Kollisionsvermeidungskriterium, Energiekriterium, Referenzlagenkriterium, Beschleunigungskriterium und einem Zusatzkriterium ist,
  • - dabei berechnet sich das Kollisionsvermeidungskriterium aus





    wobei d(q) eine Abstandsfunktion ist, die den euklidischen Abstand des geometrisch modellierten Manipulators von den geometrisch modellierten Hindernissen erfaßt, |.| die euklidische Vektornorm und ▿ den Gradientenoperator bezeichnet,
  • - das Energiekriterium aus



    (q - qi)tdiag(αj)(q - qi),

  • - das Referenzlagenkriterium aus



    (q - qref)tdiag(βj)(q - qref),



    wobei die Größe qref die Gelenkpositionen sind, die genau zwischen den zugehörigen oberen und unteren Gelenkanschlägen qmin, qmax liegen,
  • - das Beschleunigungskriterium aus



    (q - 2qi + qi-1)tdiag(γj)(q - 2qi + qi-1),

  • - das Zusatzkriterium aus



    (1 - p)2 + δorthp2orth,



    wobei die skalaren Parameter p und porth der kinematischen Gleichung gemäß



    pΔxd + porth(▿dorth, 0, 0, 0) = J(qi)(q - qi)



    und den zusätzlichen Bahnbeschränkungen 0 ≤ p ≤ 1 sowie -1 ≤ porth ≤ 1 genügen, wobei ▿dorth das Lot der orthogonalen Projektion der skalierten Hindernisoberflächennormalen auf den translatorischen Anteil der kommandierten Endeffektor-Zielverschiebung (Fig. 5) bezeichnet, p100 die prozentuale Realisierung der kommandierten Endeffektor-Zielverschiebung Δxd ist und |porth|100 die prozentuale Hindernisausweichbewegung des Endeffektors ist,
  • - ausgehend von den Größen, Gelenkpositionsversatz Δq: = q - qi = 0 bzw. Gelenkposition q = qi und p = 0, porth = 0, als Startpunkt wird auf Basis der Gütefunktion ein zulässiger Newton-Optimierungsvektor bezüglich aller aktiven Nebenbedingungen, inklusive der kinematischen Gleichung, bestimmt, die angeben, welche Bahnbeschränkungen erreicht sind, und dieser wird mit einer nichtnegativen und möglichst nahe bei eins liegenden Zahl, die aber nicht größer als eins ist, skaliert entsprechend den inaktiven Nebenbedingungen, die angeben, welche Bahnbeschränkungen nicht erreicht sind, wobei der skalierte Newton-Optimierungsvektor auf die im vorigen Iterationsschritt berechneten Größen, Gelenkpositionsversatz Δq und und p, porth, addiert wird;
  • - und auf Basis der Gütefunktion und den in den neu berechneten Größen, Gelenkpositionsversatz Δq bzw. Gelenkposition q und p, porth, aktivierten Nebenbedingungen wird die Optimalität der Größen, Gelenkpositionsversatz Δq bzw. Gelenkpositionswerte q und p, porth, bestimmt, wobei diese Optimalität durch ein globales Minimum der Gütefunktion gekennzeichnet ist, das allen Bahnbeschränkungen, inklusive der kinematischen Gleichung, genügt.

Der Algorithmus wird vorteilhaft in einer Initialisierungsphase und einer nachfolgenden Optimierungsphase abgewickelt.

Zur Vermeidung von Kollisionen zwischen Teilen des Manipulators können eben diese Teile des Manipulators selbst als Arbeitsraumhindernisse definiert werden.

Wie Fig. 2 zeigt, geht beim Verfahren nach der Erfindung in vorteilhafter Weise die Potentialgewichtung γd von dem Maximalpotential, das im euklidischen Sicherheitsabstand d0 von der durch Kollisionspunkte beschriebenen Hindernisoberfläche wirkt, zum Arbeitsraumpotential, das im euklidischen Abstand d1 > d0 von der durch Kollisionspunkte beschriebenen Hindernisoberfläche wirkt, in linearer Weise in Abhängigkeit vom euklidischen Abstand über.

Der Wert des Maximalpotentials wird dabei von einem Programmierer mindestens so groß vorgegeben, daß der Manipulator den euklidischen Sicherheitsabstand d0, der ebenfalls wie der Abstand d1 vom Programmierer vorgegeben wird, keinesfalls unterschreitet, und der nicht negative Wert des Arbeitsraumpotentials wird von einem Programmierer so vorgegeben, daß sich der Manipulator entsprechend der Form der Hindernisse vorkonfiguriert, wobei sich der Manipulator umso stärker vorkonfiguriert, je höher der Wert des Arbeitsraumpotentials ist.

Beim Verfahren nach der Erfindung wird ein Gleiten des Endeffektors auf der Hindernisoberfläche mittels einer Eigenbewegung des Endeffektors entlang des Lots der orthogonalen Projektion der mit dem Faktor





skalierten Flächennormalen ▿df, wobei |.| die euklidische Vektornorm bezeichnet, gemäß





auf die kommandierte translatorische Endeffektor-Zielverschiebung Δxt ermöglicht, wobei ▿dorth den Maximalbetrag |Δxt|prozorth hat und prozorth ≥ 0 von einem Benutzer entsprechend der maximal zulässigen Eigenbewegung vorgegeben wird, und wobei ▿df die Normale der Hindernisoberfläche (in bezug auf die beschränkende Kugel) gemäß





in dem Punkt ist, der am nächsten zur beschränkenden Kugel K(rk0, xmk0) (= Kugel, die den kleinsten Abstand zu Hindernissen hat, wobei bei Mehrdeutigkeit diejenige Kugel ausgewählt wird, die am nächsten zum Endeffektor liegt) liegt. In diesem Zusammenhang wird auch auf die Fig. 5 hingewiesen, in welcher der Hindernisausweichvektor ▿dorth dargestellt ist.

Wie Fig. 1 zeigt, wird beim Verfahren nach der Erfindung in vorteilhafter Weise mit der Hindernisorthogonalgewichtung δorth ≥ 0 die Eigenbewegung des Endeffektors entsprechend dem euklidischen Abstand von Hindernissen freigegeben, wobei die Maximalgewichtung so groß genug zu wählen ist, daß die Eigenbewegung außerhalb d2 > d1 genügend klein ist, der euklidische Abstand d2 von einem Benutzer vorgegeben wird und die Minimalgewichtung von einem Benutzer entsprechend der erforderlichen Eigenbewegung in Hindernisnähe (euklidischer Abstand ≤ d1) gewählt wird und ein linearer Übergang von Maximalgewichtung zu Minimalgewichtung in Abhängigkeit des euklidischen Abstandes gemäß Fig. 1 erfolgt.

Beim Verfahren nach der Erfindung erklärt sich der aktuelle Sollwert an die Gelenkregler des Manipulators aus q i+1|soll: = q i|soll + Δqi (mit q i|soll werden im Zeitraum ΔT die Gelenkpositionen durch die Gelenkregler entsprechend verfahren) und unter einem zulässigen Gelenkpositionsversatz wird verstanden, daß der mit Δq aktualisierte Gelenkpositionswert den physikalischen Gelenkanschlägen qmin, qmax entsprechend qmin ≤ q i|soll + Δqi ≤ qmax, Δq den Gelenkgeschwindigkeitsbeschränkungen ≙max entsprechend |Δqi| ≤ ≙maxΔT genügt, die Beschränkungen der Gelenkbeschleunigungen beim eventuellen Auftreffen von Gelenkpositionen auf ihre physikalischen Gelenkanschläge sicher eingehalten werden und die Art der Optimalität von Δqi von einem Operateur durch die Gütekriterienwichtung α, β, γ festlegbar ist.

Die Abtastzeit ΔT entsprechend ΔT ≥ max(Δtq, Δtr) ist so bemessen, daß die Rechenzeit Δtq zur Berechnung eines optimalen zulässigen Gelenkpositionsversatzes Δq und die Einregelzeit Δ tr, welche die Regler zur Angleichung der Achspositionen an die Sollwerte qsoll benötigen, innerhalb der Zeitspanne ΔT liegen.

Das Verfahren nach der Erfindung läßt sich bei der interaktiven, automatisch kollisionsvermeidenden Bahnführung eines Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel (Spacemouse) oder dergleichen von einem Operateur kommandierter Endeffektor-Zielverschiebungen in einem durch Hindernisse beschränkten Arbeitsraum anwenden.

Das Verfahren nach der Erfindung läßt sich aber auch als modularer Baustein einer übergeordneten Aufgabe mit von der übergeordneten Aufgabe kommandierten Endeffektor-Zielverschiebungen in einem durch Hindernisse beschränkten Arbeitsraum einsetzen. Eine übergeordnete Aufgabe kann beispielsweise eine solche sein, wie sie durch Kraftregelungsaufgaben gestellt ist, wobei aber durch die übergeordnete Aufgabe selber eine Kollisionsvermeidung nicht abgedeckt ist.

Die zu einem Zeitpunkt Ti kommandierte kartesische Endeffektor-Zielverschiebung kann vom Operateur und/oder einer übergeordneten Aufgabe, z. B. einer Kraftregelung, in Form eines 6-dimensionalen Inkrementvektors kommandiert werden. Alternativ kann der Inkrementvektor durch Differenzbildung absoluter Endeffektorkoordinaten bestimmt sein.

Das Verfahren nach der Erfindung und dessen Weiterbildungen ergeben ein Vorkonfigurieren eines Manipulators entsprechend der Form der Hindernisse, ermöglichen eine automatische Kollisionsvermeidung und ein Gleiten auf Hindernisoberflächen, ergeben einen nur sehr geringen Verschleiß der Antriebsvorrichtung des Manipulators durch Minimierung des lokalen Gelenkpositionsversatzes und des lokalen Gelenkgeschwindigkeitsversatzes (Energie- und Beschleunigungskriterien). Es werden schlanke Gelenkbahnen um eine Referenzlage, z. B. um die Nullage erzielt, und es wird dadurch ein reversibles Verhalten der Manipulatorbewegung erreicht. Gelenkanschläge werden in vorteilhafter Weise weitestgehend vermieden oder sanft bzw. verschleißarm angefahren.

Dieses Verhalten wird durch einen Verzögerungseffekt der Roboterachsen hervorgerufen, der proportional zum Abstand von der Referenzposition wirkt, und durch explizite Beschleunigungsbeschränkungen. Darüber hinaus entsteht ein stabiles Verhalten in singulären Roboterstellungen, da keine Invertierung der Jacobi-Matrix vorgenommen wird. Außerdem ergeben sich in vorteilhafter Weise effiziente Bahnverläufe durch exakte Übertragung der von der Handsteuerkugel kommandierten kartesischen Linearbewegung auf den Endeffektor des Manipulators.

Ferner werden Bahnbeschränkungen durch physikalische Gelenkanschläge und maximale Gelenkgeschwindigkeiten sicher eingehalten. Da hierdurch der Lösungsbereich eingeschränkt wird, wird eine hohe Konvergenzgeschwindigkeit mit einem Gewinn an Echtzeitfähigkeit erreicht. Zudem wird die prozentuale Realisierbarkeit (dexterity; Manipulierbarkeit) der kommandierten Endeffektor-Zielverschiebung berechnet.

Das Verfahren nach der Erfindung wird anhand von fünf Figuren erläutert. Es zeigen:

Fig. 1 den bereits vorher erläuterten Verlauf der Hindernisorthogonalgewichtung in Abhängigkeit vom euklidischen Hindernisabstand,

Fig. 2 den ebenfalls bereits vorher erläuterten Verlauf der Potentialgewichtung in Abhängigkeit vom euklidischen Sicherheitsabstand,

Fig. 3 die Diskretisierung der Ein-/Ausgabedatenflüsse im Echtzeitraster,

Funktionsdiagramm des Verfahrens nach der Erfindung, und

Fig. 5 eine Darstellung des Hindernisausweichvektors ▿dorth.

In Fig. 3 ist im Zusammenhang mit den vorstehend bereits ausgeführten Merkmalen des Verfahrens nach der Erfindung eine Darstellung des Ein-/Ausgabeflusses der Rückwärtskinematik im Echtzeitraster gezeigt. Dabei sind Ti-1, Ti und Ti+1 Zeitpunkte, zu denen kartesische Endeffektor-Zielverschiebungen kommandiert werden. Das Zeitintervall zwischen diesen Zeitpunkten beträgt jeweils ΔT.

Bei Erlangen der Verfahrensausgangsgrößen zu einer kommandierten Endeffektor-Zielverschiebung Δx i|d zum Zeitpunkt Ti-1 wird während des Zeitintervalls ΔT mit dem sich in einer Initialisierungsphase und einer nachfolgenden Optimierungsphase abwickelnden Algorithmus der inversen Kinematik ein zulässiger optimaler Gelenkpositionsversatz Δqi berechnet. Dabei erklärt sich der aktuelle Sollwert an die Gelenkregler aus q i+1|soll: = q i|soll + Δqi. Mit q i|soll werden im Zeitraum ΔT die Gelenkpositionen durch die Gelenkregler entsprechend verfahren.

Es folgt nun eine algorithmische Beschreibung des Verfahrens nach der Erfindung.

Es bezeichnen Ji, i = 1, . . ., ndof die Spalten der Jacobi-Matrix der Kinematik im Punkt qsoll, ndof die Anzahl der Gelenke und εi: = ≙ i|maxΔT den maximal erlaubten Gelenkpositionsversatz pro Abtastzeit ΔT. Mit





und Jε: = (J1ε1, J2ε2, . . ., Jndofεndof) lautet die inkrementelle kinematische Gleichung:



pΔxd + porth(▿dorth, 0,0,0) = Jεy.

Im weiteren Text wird ▿dorth für (▿dorth, 0,0,0) geschrieben.

Aufgrund der Bahnbeschränkungen ergeben sich folgende Boxbeschränkungen für y:





Ferner wird definiert:





Dabei bezeichnet Δqi das jüngst berechnete Inkrement.

Damit alle Bahnbeschränkungen erfüllt werden können, ist gemäß der bereits angegebenen inkrementellen kinematischen Gleichung



pΔxd + porth(▿dorth, 0, 0, 0) = Jεy



die kommandierte Endeffektor-Zielverschiebung und die Hindernisausweichbewegung jeweils einer zentrischen Streckung unterworfen, die in Form zweier Skalare beschrieben sind:



0 ≤ p ≤ 1, -1 ≤ porth ≤ 1.

Die Größen y, p und porth werden durch das Verfahren nach der Erfindung so optimiert, daß alle Beschränkungen sicher eingehalten werden. Dabei ist p100 die prozentuale Realisierbarkeit der kommandierten Endeffektor-Zielverschiebung.

Definiere den Begrenzungsvektor

b: = (0, 0, 0, 0, 0, 0, ymin, 0, -1, -ymax, -1, -1) ∈ IR2n+6 und

x: = (y, p, porth) sowie die Matrix A ∈ IR2n+6,n der Gradienten der Nebenbedingungen mit n: = ndof + 2:





Dabei bezeichnet In ∈ IRn,n die Einheitsmatrix. Ferner bezeichnen ai, i = 1, . . ., 2n + 6 die Zeilen von A.

Die Gewichte α, β, γ, δorth der Kriterien und Hindernisorthogonalgewichtung definieren den Choleskyfaktor der Hessematrix der Kriterienfunktion f(q, p, porth) in Form einer Diagonalmatrix Λ ∈ IRn,n. Ihre Hauptdiagonalelemente lauten:





Definiere die skalierte Potentialgewichtung



γ scal|d,i: = γd,iεi.

Die Grundstruktur des eine Rückwärtskinematik benutzenden besonderen Verfahrens nach der Erfindung zur Kommandosteuerung ist schematisch in Fig. 4 dargestellt.

Anhand der Fig. 4 wird im folgenden ein beim Verfahren nach der Erfindung benutzter Algorithmus für die inverse Kinematik genauer beschrieben. Beim verwendeten Algorithmus für die inverse Kinematik wird auf Basis des Konzepts der Projektionsmethoden der quadratischen Optimierung zuerst eine Initialisierungsphase durchgeführt, die folgendermaßen ausgestaltet ist:

  • - Zählindex: k = 0; Startwert: xk = (yk, pk, porth,k) = 0; kmax > 5.
  • - Initialisiere mit mk: = 7 die Matrix der Gradienten der aktiven Nebenbedingungen Ât ∈ IRmk,n:





  • - Initialisiere die Indexmenge Jk: = (j 1|k, j 2|k, . . ., j 2n+6|k) zur Kennzeichnung der aktiven, singulären und inaktiven Nebenbedingungen:





  • - Initialisiere die orthogonale Dreieckszerlegung von Â:



    (rk, Jk, Yk, Zk, Lk, Sk): = Φ A|0 (Â, mk, Jk).

  • - Initialisiere die orthogonale, reguläre Dreieckszerlegung von Z: = ΛZk:



    (Q z|k, R z|k): = Φ z|0(Z, rk).

  • - Initialisiere den Gradientenvektor der Kriterienfunktion





Für gk wird folgende Kurzschreibweise verwendet:





Beim verwendeten Algorithmus inverser Kinematik wird im Anschluß daran eine Optimierungsphase durchgeführt, die folgendermaßen ausgestaltet ist:

  • 1. Berechne die Newton-Optimierungsrichtung dk:



    R Z|kdaux = -Q Z|kZkgk.



    Bestimme hieraus daux durch Rückwärtssubstitution.



    R Z|kdZ = -Q Z|kdaux.



    Bestimme hieraus dZ durch Rückwärtssubstitution. Definiere die Optimierungsrichtung:



    dk: = -ZkdZ.

  • 2. Bestimme die maximale Schrittweite sk und den Index j i0|k der beschränkenden Nebenbedingung:





    (Bei Mehrdeutigkeiten wähle dasjenige j0 mit kleinstem Index i0.)
  • 3. Prüfe auf Optimalität und Update aller Matrix- und Indexgrößen.
    • a) Falls sk < 1 (Nebenbedingung j i0|k ist aktiv geworden):
      • a) Falls mk < n (es gibt noch inaktive Nebenbedingungen):





        • - Aktualisiere Matrixfaktorisierungen und Indexmenge:



          (rk+1, mk+1, Jk+1, Yk+1, Zk+1, Lk+1, Sk+1): = Φ A|+(i0, rk, mk, Jk, Yk, Zk, Lk, Sk).

        • - Prüfe xk+1 auf Optimalität:

          Falls Z t|k+1gk+1 = 0;

          und λk+1 = LLS(rk+1, mk+1, gk+1, Yk+1, Lk+1) ≥ 0.

          dann: Gehe zu Schritt 4.

          Sonst: k = k + 1 (erhöhe Iterationsindex)

          Falls k > kmax: Gehe zu Schritt 4.

          Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z: ΛZk+1:



          (Q Z|k+1, R Z|k+1): = ΦZ(Z, rk+1).



          Gehe zu Schritt 1.
      • b) Falls mk = n (Ecke des zulässigen Bereichs erreicht, Austausch aktiver Nebenbedingungen erforderlich):





        • - Gebe eine Richtung frei mit höchstem Optimierungspotential:





        • - Aktualisiere Matrixfaktorisierungen und Indexmenge:



          ( ≙k, ≙k, ≙k, ≙k, ≙k, ≙k, ≙k): = Φ A|-(j0, rk, mk, Jk, Yk, Zk, Lk, Sk).

        • - Aktiviere Nebenbedingung j i0|k.
        • - Aktualisiere Matrixfaktorisierungen und Indexmenge:



          (rk+1, mk+1, Jk+1, Yk+1, Zk+1, Lk+1, Sk+1): = Φ A|+(i0, ≙k, ≙k, ≙k, ≙k, ≙k, ≙k, ≙k).

        • - Prüfe xk+1 auf Optimalität:

          Falls Z t|k+1gk+1 = 0;

          und λk+1 = LLS(rk+1, mk+1, gk+1, Yk+1, Lk+1) ≥ 0.

          dann: Gehe zu Schritt 4.

          Sonst: k = k + 1 (erhöhe Iterationsindex)

          Falls k > kmax: Gehe zu Schritt 4.

          Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z: ΛZk+1:



          (Q Z|k+1, R Z|k+1): = ΦZ(Z, rk+1).



          Gehe zu Schritt 1.
    • b) Falls sk = 1 (keine neue Nebenbedingung).





      • - Aktualisiere Matrixfaktorisierungen und Indexmenge:



        (rk+1, mk+1, Jk+1, Yk+1, Zk+1, Lk+1, Sk+1): = (rk, mk, Jk, Yk, Zk, Lk, Sk).

      • - Prüfe xk+1 auf Optimalität:

        Falls Z t|k+1gk+1 = 0;

        und λk+1 = LLS(rk+1, mk+1, gk+1, Yk+1, Lk+1) ≥ 0.

        dann: Gehe zu Schritt 4.

        Sonst: Gebe eine Richtung frei mit höchstem Optimierungspotential:





      • - Aktualisiere Matrixfaktorisierungen und Indexmenge:



        (rk+1, mk+1, Jk+1, Yk+1, Zk+1, Lk+1, Sk+1): = Φ A|-(j0, rk, mk, Jk, Yk, Zk, Lk, Sk).

      • - k = k + 1 (erhöhe Iterationsindex)

        Falls k > kmax: Gehe zu Schritt 4.

        Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z: = ΛZk+1:



        (Q Z|k+1, R Z|k+1): = ΦZ(Z, rk+1).



        Gehe zu Schritt 1.
  • 4. Lösung xk+1 ermittelt. Stop!

    Ende des Algorithmus (inverse Kinematik).

    Die verwendeten Routinen LLS, Φ+, Φ-, Φ A|0 sind auch bereits in DE 198 00 552 A1 beschrieben.

    Least-square Update der Lagrangemultiplikatoren mittels Routine LLS.

    Φ+: Nebenbedingung ji0 wird aktiv mit entsprechendem Update der Faktorisierungen der Matrix der Gradienten der aktiven Nebenbedingungen.



    (r, m, J, Y, Z, L, S) = Φ A|+(i0, r, m, J, Y, Z, L, S).



    Φ_: Nebenbedingung ji0 wird inaktiv mit entsprechendem Update der Faktorisierungen der Matrix der Gradienten der aktiven Nebenbedingungen.



    (r, m, J, Y, Z, L, S) = Φ A|-(i0, r, m, J, Y, Z, L, S).

    Φ A|0: Bestimmung der Faktorisierungen der Matrix der Gradienten der aktiven Nebenbedingungen im Startpunkt.



    (r, J, Y, Z, L, S): = Φ A|0(Â, m, J).


Anspruch[de]
  1. 1. Verfahren zur automatischen Kollisionsvermeidung eines Manipulators in einem durch Hindernisse beschränkten Arbeitsraum auf der Basis eines geometrischen Weltmodells des Arbeitsbereichs und des Manipulators und von mit Hilfe einer Handsteuerkugel (Spacemouse) oder dergleichen von einem Programmierer kommandierten Endeffektor-Zielverschiebungen Δxd in Verbindung mit der Berechnung von Gelenkpositionswerten entsprechend einem Algorithmus inverser Kinematik, wobei von einem geometrischen Modell des Manipulators durch Kugeln, welche die einzelnen Teile, wie Arme und Gelenke, des Manipulators approximieren, sowie von einem geometrischen Modell der Oberflächen der Arbeitsraumhindernisse durch approximierende Punktegitter (Kollisionspunkte) ausgegangen wird, wobei das Verfahren folgende Merkmale aufweist:
    1. - der Manipulator wird durch Kugeln K(rk, xmk) mit Kugelmittelpunkten xmk ∈ IR3 und Kugelradien rk > 0 geometrisch beschrieben und die Arbeitsraumhindernisse werden durch Kollisionspunkte xcolll ∈ IR3 geometrisch beschrieben,
    2. - auf der Basis dieser Beschreibungen wird die glatte Maximum-Approximation aller Elementarabstände





      erklärt mit





      wobei der positive Parameter ρ dieser Gleichung die Güte der Maximumapproximation spezifiziert und von einem Benutzer vorgegeben wird,
    3. - ausgehend von der kommandierten Endeffektor-Zielverschiebung und dem aktuellen Istwert qi der Gelenkpositionen des Manipulators wird
    4. - unter Berücksichtigung einer zu minimierenden Gütefunktion f(q, p, porth), die durch nichtnegative Wichtungswerte αj, βj, γj, γd parametriert ist,
    5. - und unter Berücksichtigung von Bahnbeschränkungen durch physikalische Gelenkanschläge qmin, qmax, maximaler Gelenkdrehzahlen ≙max und maximaler Gelenkbeschleunigungen ≙max in einer Umgebung physikalischer Gelenkanschläge und der kinematischen Gleichung, welche durch die Jacobi-Matrix J(q) der Kinematik repräsentiert ist, eine neue Gelenkposition qi+1 berechnet, welche die neuen Werte für die Gelenkregler vorgibt, wobei die Gütefunktion f(q, p, porth) die Summe aus Kollisionsvermeidungskriterium, Energiekriterium, Referenzlagenkriterium, Beschleunigungskriterium und einem Zusatzkriterium ist,
    6. - dabei berechnet sich das Kollisionsvermeidungskriterium aus





      wobei d(q) eine Abstandsfunktion ist, die den euklidischen Abstand des geometrisch modellierten Manipulators von den geometrisch modellierten Hindernissen erfaßt, |.| die euklidische Vektornorm und ▿ den Gradientenoperator bezeichnet,
    7. - das Energiekriterium aus



      (q - qi)tdiag(αj)(q - qi),

    8. - das Referenzlagenkriterium aus



      (q - qref)tdiag(βj)(q - qref),



      wobei die Größe qref die Gelenkpositionen sind, die genau zwischen den zugehörigen oberen und unteren Gelenkanschlägen qmin, qmax liegen,
    9. - das Beschleunigungskriterium aus



      (q - 2qi + qi-1)tdiag(γj)(q - 2qi + qi-1),

    10. - das Zusatzkriterium aus



      (1 - p)2 + δorthp2orth,



      wobei die skalaren Parameter p und porth der kinematischen Gleichung gemäß



      pΔxd + porth(▿dorth, 0, 0, 0) = J(qi)(q - qi)



      und den zusätzlichen Bahnbeschränkungen 0 ≤ p ≤ 1 sowie -1 ≤ porth ≤ 1 genügen, wobei ▿dorth das Lot der orthogonalen Projektion der skalierten Hindernisoberflächennormalen auf den translatorischen Anteil der kommandierten Endeffektor-Zielverschiebung (Fig. 5) bezeichnet, p100 die prozentuale Realisierung der kommandierten Endeffektor-Zielverschiebung Δxd ist und |porth|100 die prozentuale Hindernisausweichbewegung des Endeffektors ist,
    11. - ausgehend von den Größen, Gelenkpositionsversatz Δq: = q - qi = 0 bzw. Gelenkposition q = qi und p = 0, porth = 0, als Startpunkt wird auf Basis der Gütefunktion ein zulässiger Newton-Optimierungsvektor bezüglich aller aktiven Nebenbedingungen, inklusive der kinematischen Gleichung, bestimmt, die angeben, welche Bahnbeschränkungen erreicht sind, und dieser wird mit einer nichtnegativen und möglichst nahe bei eins liegenden Zahl, die aber nicht größer als eins ist, skaliert entsprechend den inaktiven Nebenbedingungen, die angeben, welche Bahnbeschränkungen nicht erreicht sind, wobei der skalierte Newton-Optimierungsvektor auf die im vorigen Iterationsschritt berechneten Größen, Gelenkpositionsversatz Δq und und p, porth, addiert wird;
    12. - und auf Basis der Gütefunktion und den in den neu berechneten Größen, Gelenkpositionsversatz Δq bzw. Gelenkposition q und p, porth, aktivierten Nebenbedingungen wird die Optimalität der Größen, Gelenkpositionsversatz Δq bzw. Gelenkpositionswerte q und p, porth, bestimmt, wobei diese Optimalität durch ein globales Minimum der Gütefunktion gekennzeichnet ist, das allen Bahnbeschränkungen, inklusive der kinematischen Gleichung, genügt.
  2. 2. Verfahren nach Anspruch 1, dadurch gekennzeichnet, daß zur Vermeidung von Kollisionen zwischen Teilen des Manipulators eben diese Teile des Manipulators selbst als Arbeitsraumhindernisse definiert werden.
  3. 3. Verfahren nach Anspruch 1 oder 2, dadurch gekennzeichnet, daß die Potentialgewichtung γd von dem Maximalpotential, das im euklidischen Sicherheitsabstand d0 von der durch Kollisionspunkte beschriebenen Hindernisoberfläche wirkt, zum Arbeitsraumpotential, das im euklidischen Abstand d1 > d0 von der durch Kollisionspunkte beschriebenen Hindernisoberfläche wirkt, in linearer Weise in Abhängigkeit vom euklidischen Abstand übergeht.
  4. 4. Verfahren nach Anspruch 3, dadurch gekennzeichnet, daß der Wert des Maximalpotentials von einem Programmierer mindestens so groß vorgegeben wird, daß der Manipulator den euklidischen Sicherheitsabstand d0, der ebenfalls wie der Abstand d1 vom Programmierer vorgegeben wird, keinesfalls unterschreitet, und der nicht negative Wert des Arbeitsraumpotentials wird von einem Programmierer so vorgegeben, daß sich der Manipulator entsprechend der Form der Hindernisse vorkonfiguriert, wobei sich der Manipulator umso stärker vorkonfiguriert, je höher der Wert des Arbeitsraumpotentials ist.
  5. 5. Verfahren nach einem der vorhergehenden Ansprüche, dadurch gekennzeichnet, daß ein Gleiten des Endeffektors auf der Hindernisoberfläche mittels einer Eigenbewegung des Endeffektors entlang des Lots der orthogonalen Projektion der mit dem Faktor





    skalierten Flächennormalen ▿df, wobei |.| die euklidische Vektornorm bezeichnet, gemäß





    auf die kommandierte translatorische Endeffektor-Zielverschiebung Δxt ermöglicht wird, wobei ▿dorth den Maximalbetrag |Δxt|prozorth hat und prozorth ≥ 0 von einem Benutzer entsprechend der maximal zulässigen Eigenbewegung vorgegeben wird, und wobei ▿df die Normale der Hindernisoberfläche (in bezug auf die beschränkende Kugel) gemäß





    in dem Punkt ist, der am nächsten zur beschränkenden Kugel K(rk0, xmk0) (= Kugel, die den kleinsten Abstand zu Hindernissen hat, wobei bei Mehrdeutigkeit diejenige Kugel ausgewählt wird, die am nächsten zum Endeffektor liegt) liegt.
  6. 6. Verfahren nach einem der vorhergehenden Ansprüche, dadurch gekennzeichnet, daß mit der Hindernisorthogonalgewichtung δorth ≥ 0 die Eigenbewegung des Endeffektors entsprechend dem euklidischen Abstand von Hindernissen freigegeben wird, wobei die Maximalgewichtung so groß genug zu wählen ist, daß die Eigenbewegung außerhalb d2 > d1 genügend klein ist, der euklidische Abstand d2 von einem Benutzer vorgegeben wird und die Minimalgewichtung von einem Benutzer entsprechend der erforderlichen Eigenbewegung in Hindernisnähe (euklidischer Abstand ≤ d1) gewählt wird und ein linearer Übergang von Maximalgewichtung zu Minimalgewichtung in Abhängigkeit des euklidischen Abstandes erfolgt.
  7. 7. Verfahren nach Anspruch 1, dadurch gekennzeichnet, daß sich der aktuelle Sollwert an die Gelenkregler des Manipulators aus q i+1|soll: = q i|soll + Δq i|soll erklärt (mit q i|soll werden im Zeitraum ΔT die Gelenkpositionen durch die Gelenkregler entsprechend verfahren) und unter einem zulässigen Gelenkpositionsversatz verstanden wird, daß der mit Δq aktualisierte Gelenkpositionswert den physikalischen Gelenkanschlägen qmin, qmax entsprechend qmin ≤ q i|soll + Δqi ≤ qmax, Δq den Gelenkgeschwindigkeitsbeschränkungen ≙max entsprechend |Δqi| ≤ ≙maxΔT genügt, die Beschränkungen der Gelenkbeschleunigungen beim eventuellen Auftreffen von Gelenkpositionen auf ihre physikalischen Gelenkanschläge sicher eingehalten werden und die Art der Optimalität von Δqi von einem Operateur durch die Gütekriterienwichtung α, β, γ festlegbar ist.
  8. 8. Verfahren nach Anspruch 7, dadurch gekennzeichnet, daß die Abtastzeit ΔT entsprechend ΔT ≥ max(Δtq, Δtr) so bemessen ist, daß die Rechenzeit Δtq zur Berechnung eines optimalen zulässigen Gelenkpositionsversatzes Δq und die Einregelzeit Δtr, welche die Regler zur Angleichung der Achspositionen an die Sollwerte qsoll benötigen, innerhalb der Zeitspanne ΔT liegen.
  9. 9. Verfahren nach einem der vorhergehenden Ansprüche, gekennzeichnet durch eine Anwendung bei der interaktiven, automatisch kollisionsvermeidenden Bahnführung eines Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel (Spacemouse) oder dergleichen von einem Operateur kommandierter Endeffektor-Zielverschiebungen in einem durch Hindernisse beschränkten Arbeitsraum.
  10. 10. Verfahren nach einem der Ansprüche 1 bis 8, gekennzeichnet durch die Anwendung als modularer Baustein einer übergeordneten Aufgabe mit von der übergeordneten Aufgabe kommandierten Endeffektor-Zielverschiebungen in einem durch Hindernisse beschränktem Arbeitsraum.
  11. 11. Verfahren nach Anspruch 10, dadurch gekennzeichnet, daß eine übergeordnete Aufgabe eine solche ist, wie sie durch Kraftregelungsaufgaben gestellt ist, wobei aber durch die übergeordnete Aufgabe selber eine Kollisionsvermeidung nicht abgedeckt ist.
  12. 12. Verfahren nach Anspruch 10 und/oder 11, dadurch gekennzeichnet, daß die zu einem Zeitpunkt Ti kommandierte kartesische Endeffektor-Zielverschiebung vom Operateur und/oder einer übergeordneten Aufgabe, z. B. einer Kraftregelung, in Form eines 6-dimensionalen Inkrementvektors kommandiert wird oder alternativ hierzu der Inkrementvektor durch Differenzbildung absoluter Endeffektorkoordinaten bestimmt wird.
  13. 13. Verfahren nach Anspruch 1, dadurch gekennzeichnet, daß der angewandte Algorithmus der inversen Kinematik in einer Initialisierungsphase und einer nachfolgenden Optimierungsphase abgewickelt wird.






IPC
A Täglicher Lebensbedarf
B Arbeitsverfahren; Transportieren
C Chemie; Hüttenwesen
D Textilien; Papier
E Bauwesen; Erdbohren; Bergbau
F Maschinenbau; Beleuchtung; Heizung; Waffen; Sprengen
G Physik
H Elektrotechnik

Anmelder
Datum

Patentrecherche

Patent Zeichnungen (PDF)

Copyright © 2008 Patent-De Alle Rechte vorbehalten. eMail: info@patent-de.com