DE19810341A1 - Verfahren zur automatischen Kollisionsvermeidung eines Manipulators in einem durch Hindernisse beschränkten Arbeitsraum - Google Patents

Abstract

Beim Verfahren nach der Erfindung erfolgt ein Vorkonfigurieren des Manipulators entsprechend der Form der Hindernisse, wodurch eine automatische Kollisionsvermeidung und ein Gleiten auf Hindernisoberflächen erreicht wird. Das besondere Verhalten wird beim Verfahren durch einen Verzögerungseffekt der Manipulatorachsen, der proportional zum Abstand von der Referenzposition wirkt, und durch explizite Beschleunigungsbeschränkungen hervorgerufen. Es werden Energie- und Beschleunigungskriterien angegeben, die eine Minimierung des lokalen Gelenkpositionsversatzes und des lokalen Geschwindigkeitsversatzes zur Folge haben und somit zu einem geringen Verschleiß des Manipulatorantriebs führen. Das Verfahren nach der Erfindung läßt sich bei Manipulatoren und Robotern anwenden.

Description

Die Erfindung bezieht sich auf ein Verfahren zur automati­ schen Kollisionsvermeidung eines Manipulators in einem durch Hindernisse beschränkten Arbeitsraum auf der Basis eines geo­ metrischen Weltmodells des Arbeitsbereichs und des Manipula­ tors und von mit Hilfe einer Handsteuerkugel (Spacemouse) oder dergleichen von einem Programmierer kommandierten Endef­ fektor-Zielverschiebungen Δx_d in Verbindung mit der Berech­ nung von Gelenkpositionswerten entsprechend einem Algorith­ mus, wobei von einem geometrischen Modell des Manipulators durch Kugeln, welche die einzelnen Teile, wie Arme und Gelen­ ke, des Manipulators approximieren, sowie von einem geometri­ schen 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-Ver­ lag, Tokyo, 1986, Seiten 237 bis 257 sind Verfahren zur automatischen Kollisionsvermeidung eines Manipulators auf der Grundlage kommandierter Endeffektor-Zielvorgaben in Verbin­ dung mit einer Berechnung entsprechend einem Algorithmus in­ verser 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 verallgemeiner­ ten Inversen der Jacobi-Matrix bekannt, mit denen Kollisions­ vermeidung durch Maximierung einer Abstandsfunktion als glo­ bales Kriterium im Nullraum der Jacobi-Matrix oder durch eine gewichtete, auf Basis der Ausdehnungen der Hindernisse im Ar­ beitsraum ausgerichtete Optimierung des zu berechnenden aktu­ ellen Gelenkpositionsversatzes realisiert ist.

Von Nachteil ist hierbei, daß Bahnbeschränkungen durch physi­ kalische Gelenkanschläge und maximale Gelenkgeschwindigkeiten nicht sicher eingehalten werden können, womit Bahnfehler des kommandierten Endeffektor-Ziels in einer Umgebung solcher Be­ schränkungen auftreten, sich ein instabiles Verhalten in sin­ gulären Roboterstellungen durch eine (verallgemeinerte) In­ vertierung der Jacobi-Matrix ergibt und ineffiziente Roboter­ bahnverläufe möglich sind, wenn widersprechende lokale und globale Kriterien auftreten.

Der Erfindung liegt vor allem die Aufgabe zugrunde, ein Ver­ fahren mit automatischer Kollisionsvermeidung zur Kommando­ steuerung eines Manipulators zu schaffen, mit dem Bahnbe­ schränkungen durch physikalische Gelenkanschläge sowie maxi­ mal erlaubte Gelenkgeschwindigkeiten sicher eingehalten wer­ den, mit dem eine minimale Belastung auf die Antriebsvorrich­ tung des Manipulators durch eine Optimierung des Beschleuni­ gungsverhaltens 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 automati­ schen Kollisionsvermeidung eines Manipulators der eingangs genannten Art bezieht, wird diese Aufgabe durch die Anwendung folgender Merkmale gelöst:

• - der Manipulator wird durch Kugeln K(r_k,xm_k) mit Kugelmit­ telpunkten xm_k∈R ³ und Kugelradien r_k<0 geometrisch be­ schrieben und die Arbeitsraumhindernisse werden durch Kollisionspunkte xcoll₁∈R ³ geometrisch beschrieben,
• - auf der Basis dieser Beschreibungen wird die glatte Maxi­ mum-Approximation aller Elementarabstände
  erklärt mit
  wobei der positive Parameter ρ dieser Gleichung die Güte der Maximumapproximation spezifiziert und von einem Be­ nutzer vorgegeben wird,
• - ausgehend von der kommandierten Endeffektor-Zielverschie­ bung und dem aktuellen Istwert q_i der Gelenkpositionen des Manipulators wird
  • - unter Berücksichtigung einer zu minimierenden Güte­ funktion f(q,p,p_orth), die durch nichtnegative Wich­ tungswerte α_j, β_j, γ_j, γ_d parametriert ist,
  • - und unter Berücksichtigung von Bahnbeschränkungen durch physikalische Gelenkanschläge q_min, q_max, maxi­ maler Gelenkdrehzahlen _max und maximaler Gelenkbe­ schleunigungen _max in einer Umgebung physikalischer Gelenkanschläge und der kinematischen Gleichung, wel­ che durch die Jacobi-Matrix J(q) der Kinematik reprä­ sentiert ist,
  eine neue Gelenkposition q_i+1 berechnet, welche die neuen Werte für die Gelenkregler vorgibt, wobei die Gütefunk­ tion f(q,p,p_orth) die Summe aus Kollisionsvermeidungskri­ terium, Energiekriterium, Referenzlagenkriterium, Be­ schleunigungskriterium und einem Zusatzkriterium ist,
  • - dabei berechnet sich das Kollisionsvermeidungskrite­ rium aus
    wobei d(q) eine Abstandsfunktion ist, die den eukli­ dischen Abstand des geometrisch modellierten Manipu­ lators von den geometrisch modellierten Hindernissen erfaßt, |.| die euklidische Vektornorm und ∇ den Gra­ dientenoperator bezeichnet,
  • - das Energiekriterium aus
    (q - q_i)^tdiag(α_j) (q - q_i),
  • - das Referenzlagenkriterium aus
    (q - q_ref)^tdiag(β_j) (q - q_ref),
    wobei die Größe q_ref die Gelenkpositionen sind, die genau zwischen den zugehörigen oberen und unteren Ge­ lenkanschlägen q_min, q_max liegen,
  • - das Beschleunigungskriterium aus
    (q - 2q_i + q_i-1)^tdiag(γ_j) (q - 2q_i + q_i-1),
  • - das Zusatzkriterium aus
    (1 - p)² + δ_orthp² _orth,
    wobei die skalaren Parameter p und p_orth der kinema­ tischen Gleichung gemäß
    pΔx_d + p_orth(∇d_orth,0,0,0) = J(q_i) (q - q_i)
    und den zusätzlichen Bahnbeschränkungen 0≦p≦1 so­ wie -1≦p_orth≦1 genügen, wobei ∇d_orth das Lot der orthogonalen Projektion der skalierten Hindernisober­ flächennormalen auf den translatorischen Anteil der kommandierten Endeffektor-Zielverschiebung (Fig. 5 bezeichnet, p100 die prozentuale Realisierung der kommandierten Endeffektor-Zielverschiebung Δx_d ist und |p_orth|100 die prozentuale Hindernisausweichbewe­ gung des Endeffektors ist,
• - ausgehend von den Größen, Gelenkpositionsversatz Δq:= q-q_i=0 bzw. Gelenkposition q=q_i und p=0, p_orth=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änkun­ gen erreicht sind, und dieser wird mit einer nichtnegati­ ven und möglichst nahe bei eins liegenden Zahl, die aber nicht größer als eins ist, skaliert entsprechend den in­ aktiven Nebenbedingungen, die angeben, welche Bahnbe­ schränkungen nicht erreicht sind, wobei der skalierte Newton-Optimierungsvektor auf die im vorigen Iterations­ schritt berechneten Größen, Gelenkpositionsversatz Δq und und p, p_orth, addiert wird;
• - und auf Basis der Gütefunktion und den in den neu berech­ neten Größen, Gelenkpositionsversatz Δq bzw. Gelenkposi­ tion q und p, p_orth, aktivierten Nebenbedingungen wird die Optimalität der Größen, Gelenkpositionsversatz Δq bzw. Gelenkpositionswerte q und p, p_orth, bestimmt, wobei diese Optimalität durch ein globales Minimum der Güte­ funktion gekennzeichnet ist, das allen Bahnbeschränkun­ gen, inklusive der kinematischen Gleichung, genügt.

Der Algorithmus wird vorteilhaft in einer Initialisierungs­ phase und einer nachfolgenden Optimierungsphase abgewickelt.

Zur Vermeidung von Kollisionen zwischen Teilen des Manipula­ tors können eben diese Teile des Manipulators selbst als Ar­ beitsraumhindernisse definiert werden.

Wie Fig. 2 zeigt, geht beim Verfahren nach der Erfindung in vorteilhafter Weise die Potentialgewichtung γ_d von dem Maxi­ malpotential, das im euklidischen Sicherheitsabstand d₀ von der durch Kollisionspunkte beschriebenen Hindernisoberfläche wirkt, zum Arbeitsraumpotential, das im euklidischen Abstand d₁<d₀ von der durch Kollisionspunkte beschriebenen Hindernis­ oberfläche wirkt, in linearer Weise in Abhängigkeit vom eu­ klidischen Abstand über.

Der Wert des Maximalpotentials wird dabei von einem Program­ mierer mindestens so groß vorgegeben, daß der Manipulator den euklidischen Sicherheitsabstand d₀, der ebenfalls wie der Ab­ stand d₁ vom Programmierer vorgegeben wird, keinesfalls un­ terschreitet, und der nicht negative Wert des Arbeitsraumpo­ tentials wird von einem Programmierer so vorgegeben, daß sich der Manipulator entsprechend der Form der Hindernisse vorkon­ figuriert, wobei sich der Manipulator umso stärker vorkonfi­ guriert, je höher der Wert des Arbeitsraumpotentials ist.

Beim Verfahren nach der Erfindung wird ein Gleiten des Endef­ fektors auf der Hindernisoberfläche mittels einer Eigenbewe­ gung des Endeffektors entlang des Lots der orthogonalen Pro­ jektion der mit dem Faktor

skalierten Flächennormalen ∇d_f, wobei |.| die euklidische Vektornorm bezeichnet, gemäß

auf die kommandierte translatorische Endeffektor-Zielver­ schiebung Δx_t ermöglicht, wobei ∇d_orth den Maximalbetrag |Δx_t|proz_orth hat und proz_orth≧0 von einem Benutzer entspre­ chend der maximal zulässigen Eigenbewegung vorgegeben wird, und wobei ∇d_f 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(r_k0,xm_k0) (= Kugel, die den kleinsten Abstand zu Hindernis­ sen 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 ∇d_orth 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 Eigen­ bewegung außerhalb d₂<d₁ genügend klein ist, der euklidische Abstand d₂ von einem Benutzer vorgegeben wird und die Mini­ malgewichtung von einem Benutzer entsprechend der erforderli­ chen Eigenbewegung in Hindernisnähe (euklidischer Ab­ stand ≦d_t) gewählt wird und ein linearer Übergang von Maxi­ malgewichtung zu Minimalgewichtung in Abhängigkeit des eukli­ dischen 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+Δqⁱ (mit q i|soll werden im Zeitraum ΔT die Gelenk­ positionen durch die Gelenkregler entsprechend verfahren) und unter einem zulässigen Gelenkpositionsversatz wird verstan­ den, daß der mit Δq aktualisierte Gelenkpositionswert den physikalischen Gelenkanschlägen q_min, q_max entsprechend q_min≦q i|soll+Δqⁱ≦q_max, Δq den Gelenkgeschwindigkeitsbeschrän­ kungen _max entsprechend |Δqⁱ≦_maxΔT genügt, die Beschrän­ kungen der Gelenkbeschleunigungen beim eventuellen Auftreffen von Gelenkpositionen auf ihre physikalischen Gelenkanschläge sicher eingehalten werden und die Art der Optimalität von Δq_i von einem Operateur durch die Gütekriterienwichtung α,β,γ festlegbar ist.

Die Abtastzeit ΔT entsprechend ΔT≧max(Δt_q, Δt_r) ist so be­ messen, daß die Rechenzeit Δt_q zur Berechnung eines optimalen zulässigen Gelenkpositionsversatzes Δq und die Einregelzeit Δt_r, welche die Regler zur Angleichung der Achspositionen an die Sollwerte q_soll benötigen, innerhalb der Zeitspanne ΔT liegen.

Das Verfahren nach der Erfindung läßt sich bei der interakti­ ven, automatisch kollisionsvermeidenden Bahnführung eines Ma­ nipulators auf der Basis von mit Hilfe einer Handsteuerkugel (Spacemouse) oder dergleichen von einem Operateur komman­ dierter Endeffektor-Zielverschiebungen in einem durch Hinder­ nisse beschränkten Arbeitsraum anwenden.

Das Verfahren nach der Erfindung läßt sich aber auch als mo­ dularer Baustein einer übergeordneten Aufgabe mit von der übergeordneten Aufgabe kommandierten Endeffektor-Zielver­ schiebungen in einem durch Hindernisse beschränkten Arbeits­ raum einsetzen. Eine übergeordnete Aufgabe kann beispielswei­ se eine solche sein, wie sie durch Kraftregelungsaufgaben ge­ stellt ist, wobei aber durch die übergeordnete Aufgabe selber eine Kollisionsvermeidung nicht abgedeckt ist.

Die zu einem Zeitpunkt T_i kommandierte kartesische Endeffek­ tor-Zielverschiebung kann vom Operateur und/oder einer über­ geordneten Aufgabe, z. B. einer Kraftregelung, in Form eines 6-dimensionalen Inkrementvektors kommandiert werden. Alterna­ tiv 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 Kol­ lisionsvermeidung und ein Gleiten auf Hindernisoberflächen, ergeben einen nur sehr geringen Verschleiß der Antriebsvor­ richtung des Manipulators durch Minimierung des lokalen Ge­ lenkpositionsversatzes und des lokalen Gelenkgeschwindig­ keitsversatzes (Energie- und Beschleunigungskriterien). Es werden schlanke Gelenkbahnen um eine Referenzlage, z. B. um die Nullage erzielt, und es wird dadurch ein reversibles Ver­ halten der Manipulatorbewegung erreicht. Gelenkanschläge wer­ den in vorteilhafter Weise weitestgehend vermieden oder sanft bzw. verschleißarm angefahren.

Dieses Verhalten wird durch einen Verzögerungseffekt der Ro­ boterachsen hervorgerufen, der proportional zum Abstand von der Referenzposition wirkt, und durch explizite Beschleuni­ gungsbeschränkungen. Darüber hinaus entsteht ein stabiles Verhalten in singulären Roboterstellungen, da keine Invertie­ rung der Jacobi-Matrix vorgenommen wird. Außerdem ergeben sich in vorteilhafter Weise effiziente Bahnverläufe durch ex­ akte Übertragung der von der Handsteuerkugel kommandierten kartesischen Linearbewegung auf den Endeffektor des Manipula­ tors.

Ferner werden Bahnbeschränkungen durch physikalische Ge­ lenkanschläge und maximale Gelenkgeschwindigkeiten sicher eingehalten. Da hierdurch der Lösungsbereich eingeschränkt wird, wird eine hohe Konvergenzgeschwindigkeit mit einem Ge­ winn an Echtzeitfähigkeit erreicht. Zudem wird die prozentua­ le Realisierbarkeit (dexterity; Manipulierbarkeit) der kom­ mandierten 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 Hinder­ nisorthogonalgewichtung in Abhängigkeit vom eukli­ dischen Hindernisabstand,

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

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

Fig. 4 ein Funktionsdiagramm des Verfahrens nach der Er­ findung, und

Fig. 5 eine Darstellung des Hindernisausweichvektors ∇d_orth.

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

Bei Erlangen der Verfahrensausgangsgrößen zu einer komman­ dierten Endeffektor-Zielverschiebung Δx i|d zum Zeitpunkt T_i-1 wird während des Zeitintervalls ΔT mit dem sich in einer In­ itialisierungsphase und einer nachfolgenden Optimierungsphase abwickelnden Algorithmus der inversen Kinematik ein zulässi­ ger optimaler Gelenkpositionsversatz Δqⁱ berechnet. Dabei er­ klärt sich der aktuelle Sollwert an die Gelenkregler aus q i+1|soll: = q i|soll+Δqⁱ. Mit q i|soll werden im Zeitraum ΔT die Gelenk­ positionen durch die Gelenkregler entsprechend verfahren.

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

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

und J_ε: = (J₁ε₁, J₂ε₂, . . ., J_nfodε_ndof) lautet die inkrementelle kinematische Gleichung:

pΔx_d + p_orth(∇d_orth,0,0,0) = J_εy.

Im weiteren Text wird ∇d_orth für (∇d_orth,0,0,0) geschrieben.

Aufgrund der Bahnbeschränkungen ergeben sich folgende Boxbe­ schränkungen für y:

mit

Ferner wird definiert:

Dabei be­ zeichnet Δqⁱ das jüngst berechnete Inkrement.

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

pΔx_d + p_orth(∇d_orth,0,0,0) = J_εy

die kommandierte Endeffektor-Zielverschiebung und die Hinder­ nisausweichbewegung jeweils einer zentrischen Streckung un­ terworfen, die in Form zweier Skalare beschrieben sind:

0 ≦ p ≦ 1, -1 ≦ p_orth ≦ 1.

Die Größen y, p und p_orth werden durch das Verfahren nach der Erfindung so optimiert, daß alle Beschränkungen sicher einge­ halten werden. Dabei ist p100 die prozentuale Realisierbar­ keit der kommandierten Endeffektor-Zielverschiebung.

Definiere den Begrenzungsvektor

b:= (0,0,0,0,0,0,y_min,0,-1,-y_max,-1,-1)∈R ²ⁿ⁺⁶ und
x:= (y,p,p_orth) sowie die Matrix A∈R ^2n+6,n der Gradien­ ten der Nebenbedingungen mit n:= ndof + 2:

Dabei bezeichnet I_n∈R ^n,n die Einheitsmatrix. Ferner be­ zeichnen a_i, i=1, . . ., 2n+6 die Zeilen von A.

Die Gewichte α,β,γ,δ_orth der Kriterien und Hindernisorthogo­ nalgewichtung definieren den Choleskyfaktor der Hessematrix der Kriterienfunktion f(q,p,p_orth) in Form einer Diagonalma­ trix Λ∈R ^n,n. Ihre Hauptdiagonalelemente lauten:

und

mit

Definiere die skalierte Potentialgewichtung

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

Die Grundstruktur des eine Rückwärtskinematik benutzenden be­ sonderen 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 in­ verse Kinematik wird auf Basis des Konzepts der Projek­ tionsmethoden der quadratischen Optimierung zuerst eine In­ itialisierungsphase durchgeführt, die folgendermaßen ausge­ staltet ist:

• - Zählindex: k=0; Startwert: x_k=(y_k,p_k,p_orth,k)=0; k_max<5.
• - Initialisiere mit m_k:=7 die Matrix der Gradienten der aktiven Nebenbedingungen Â^t∈R ^m_k,n:
  
• - Initialisiere die Indexmenge J_k:=(j 1|k, j 2|k, . . . j 2n+6|k) zur Kennzeichnung der aktiven, singulären und inaktiven Ne­ benbedingungen:
  
• - Initialisiere die orthogonale Dreieckszerlegung von Â:
  (r_k, J_k, Y_k, Z_k, L_k, S_k):=Φ A|0(Â, m_k, J_k).
• - Initialisiere die orthogonale, reguläre Dreieckszerlegung von Z:= ΛZ_k
  (Q Z|k, R Z|k): = Φ Z|0(Z, r_k).
• - Initialisiere den Gradientenvektor der Kriterienfunktion
  Für g_k wird folgende Kurzschreibweise verwendet:
  

Beim verwendeten Algorithmus inverser Kinematik wird im An­ schluß daran eine Optimierungsphase durchgeführt, die folgen­ dermaßen ausgestaltet ist:

• 1. Berechne die Newton-Optimierungsrichtung d_k:
  R Z|kd_aux = -Q Z|kZ_kg_k.
  Bestimme hieraus d_aux durch Rückwärtssubstitution.
  R Z|kd_Z = -Q Z|kd_aux.
  Bestimme hieraus d_Z durch Rückwärtssubstitution. Defi­ niere die Optimierungsrichtung:
  d_k := Z_kd_Z.
• 2. Bestimme die maximale Schrittweite s_k und den Index j i0|k der beschränkenden Nebenbedingung:
  (Bei Mehrdeutigkeiten wähle dasjenige j₀ mit kleinstem Index i₀.)
• 3. Prüfe auf Optimalität und Update aller Matrix- und In­ dexgrößen.
  • (a) Falls s_k<1 (Nebenbedingung j i0|k ist aktiv geworden):
  • i. Falls m_k<n (es gibt noch inaktive Nebenbedingun­ gen):
    
    • - Aktualisiere Matrixfaktorisierungen und Indexmen­ ge:
      (r_k+1, m_k+1, J_k+1, Y_k+1, Z_k+1, L_k+1, S_k+1): = Φ A|+(i0, r_k, m_k, J_k, Y_k, Z_k, L_k, S_k).
    • - Prüfe x_k+1 auf Optimalität:
      Falls Z t|k+1g_k+1 = 0;
      und λ_k+1=LLS(r_k+1,m_k+1,g_k+1,Y_k+1,L_k+1) ≧ 0.
      dann: Gehe zu Schritt 4.
      Sonst: k=k+1 (erhöhe Iterationsindex).
      Falls k<k_max: Gehe zu Schritt 4.
      Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z: ΛZ_k+1:
      (Q Z|k+1, R Z|k+1) := Φ^z(Z, r_k+1).
      Gehe zu Schritt 1.
  • ii. Falls m_k=n (Ecke des zulässigen Bereichs er­ reicht, Austausch aktiver Nebenbedingungen erfor­ derlich):
    
    • - Gebe eine Richtung frei mit höchstem Optimie­ rungspotential:
      
    • - Aktualisiere Matrixfaktorisierungen und Indexmen­ ge:
      (_k, _k, _k, _k, _k, _k, _k): = Φ A|_(j0, r_k, m_k, J_k, Y_k, Z_k, L_k, S_k).
    • - Aktiviere Nebenbedingung j i0|k.
    • - Aktualisiere Matrixfaktorisierungen und Indexmen­ ge:
      (r_k+1, m_k+1, J_k+1, Y_k+1, Z_k+1, L_k+1, S_k+1): = Φ A|+(i0, _k, _k, _k, _k, _k, _k, _k).
    • - Prüfe x_k+1 auf Optimalität:
      Falls Z t|k+1g_k+1 = 0;
      und λ_k+1=LLS(r_k+1,m_k+1,g_k+1,Y_k+1,L_k+1) ≧ 0.
      dann: Gehe zu Schritt 4.
      Sonst: k=k+1 (erhöhe Iterationsindex),
      Falls k<k_max: Gehe zu Schritt 4.
      Sonst: Bestimme die orthogonale, reguläre Drei­ eckszerlegung von Z: ΛZ_k+1:
      (Q Z|k+1, R Z|k+1): = Φ^z(Z, r_k+1).
      Gehe zu Schritt 1.
  • (b) Falls s_k=1 (keine neue Nebenbedingung).
    
    • - Aktualisiere Matrixfaktorisierungen und Indexmen­ ge:
      (r_k+1, m_k+1, J_k+1, Y_k+1, Z_k+1, L_k+1, S_k+1): = (r_k, m_k, J_k, Y_k, Z_k, L_k, S_k).
    • - Prüfe x_k+1 auf Optimalität:
      Falls Z t|k+1g_k+1 = 0;
      und λ_k+1=LLS(r_k+1,m_k+1,g_k+1,Y_k+1,L_k+1) ≧ 0.
      dann: Gehe zu Schritt 4.
      Sonst: Gebe eine Richtung frei mit höchstem Op­ timierungspotential:
      
      • - Aktualisiere Matrixfaktorisierungen und Index­ menge:
        (r_k+1, m_k+1, J_k+1, Y_k+1, Z_k+1, L_k+1, S_k+1): = Φ A|_(j0, r_k, m_k, J_k, Y_k, Z_k, L_k, S_k).
      • - k = k+1 (erhöhe Iterationsindex)
        Falls k<k_max: Gehe zu Schritt 4.
        Sonst: Bestimme die orthogonale, reguläre Drei­ eckszerlegung von Z: = ΛZ_k+1:
        (Q^z _k+1, R^z _k+1): = Φ^z(Z, r_k+1).
        Gehe zu Schritt 1.
• 4. Lösung x_k+1 ermittelt. Stop!

Ende des Algorithmus (inverse Kinematik).

Die verwendeten Routinen LLS, Φ₊, Φ_-, Φ A|0 sind auch bereits in der deutschen Patentanmeldung P 197 26 166.3-32 beschrie­ ben.

Least-square Update der Lagrangemultiplikatoren mittels Rou­ tine LLS.

Φ₊: Nebenbedingung j^i₀ 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 jⁱ0 wird inaktiv mit entsprechendem Up­ date der Faktorisierungen der Matrix der Gradienten der akti­ ven 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 Gradien­ ten der aktiven Nebenbedingungen im Startpunkt.

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

Claims (13)

1. Verfahren zur automatischen Kollisionsvermeidung eines Ma­ nipulators in einem durch Hindernisse beschränkten Arbeits­ raum auf der Basis eines geometrischen Weltmodells des Ar­ beitsbereichs und des Manipulators und von mit Hilfe einer Handsteuerkugel (Spacemouse) oder dergleichen von einem Pro­ grammierer kommandierten Endeffektor-Zielverschiebungen Δx_d in Verbindung mit der Berechnung von Gelenkpositionswerten entsprechend einem Algorithmus, 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 Ar­ beitsraumhindernisse durch approximierende Punktegitter (Kollisionspunkte) ausgegangen wird, gekennzeichnet durch folgende Merkmale:

• - der Manipulator wird durch Kugeln K(r_k,xm_k) mit Kugelmit­ telpunkten xm_k∈R ³ und Kugelradien r_k<0 geometrisch be­ schrieben und die Arbeitsraumhindernisse werden durch Kollisionspunkte xcoll₁∈R ³ geometrisch beschrieben,
• - auf der Basis dieser Beschreibungen wird die glatte Maxi­ mum-Approximation aller Elementarabstände
  erklärt mit
  wobei der positive Parameter ρ dieser Gleichung die Güte der Maximumapproximation spezifiziert und von einem Be­ nutzer vorgegeben wird,
• - ausgehend von der kommandierten Endeffektor-Zielverschie­ bung und dem aktuellen Istwert q_i der Gelenkpositionen des Manipulators wird,
  • - unter Berücksichtigung einer zu minimierenden Güte­ funktion f(q,p,p_orth), die durch nichtnegative Wich­ tungswerte α_j, β_j, γ_j, γ_d parametriert ist,
  • - und unter Berücksichtigung von Bahnbeschränkungen durch physikalische Gelenkanschläge q_min, q_maxi maxi­ maler Gelenkdrehzahlen _max und maximaler Gelenkbe­ schleunigungen _max in einer Umgebung physikalischer Gelenkanschläge und der kinematischen Gleichung, wel­ che durch die Jacobi-Matrix J(q) der Kinematik reprä­ sentiert ist,
    eine neue Gelenkposition q_i+1 berechnet, welche die neuen Werte für die Gelenkregler vorgibt, wobei die Gütefunk­ tion f(q,p,p_orth) die Summe aus Kollisionsvermeidungskri­ terium, Energiekriterium, Referenzlagenkriterium, Be­ schleunigungskriterium und einem Zusatzkriterium ist,
  • - dabei berechnet sich das Kollisionsvermeidungskrite­ rium aus
    wobei d(q) eine Abstandsfunktion ist, die den eukli­ dischen Abstand des geometrisch modellierten Manipu­ lators von den geometrisch modellierten Hindernissen erfaßt, |.| die euklidische Vektornorm und ∇ den Gra­ dientenoperator bezeichnet,
  • - das Energiekriterium aus
    (q - q_i)^tdiag(α_j) (q - q_i),
  • - das Referenzlagenkriterium aus
    (q - q_ref)^tdiag(β_j) (q - q_ref),
    wobei die Größe q_ref die Gelenkpositionen sind, die genau zwischen den zugehörigen oberen und unteren Ge­ lenkanschlägen q_min, q_max liegen,
  • - das Beschleunigungskriterium aus
    (q - 2q_i + q_i-1)^tdiag(γ_j) (q - 2q_i + q_i-1),
  • - das Zusatzkriterium aus
    (1 - p)² + δ_orthp² _Zorth,
    wobei die skalaren Parameter p und p_orth der kinema­ tischen Gleichung gemäß
    pΔx_d + p_orth(∇d_orth,0,0,0) = J(q_i) (q - q_i)
    und den zusätzlichen Bahnbeschränkungen 0≦p≦1 so­ wie -1≦p_orth≦1 genügen, wobei ∇d_orth das Lot der orthogonalen Projektion der skalierten Hindernisober­ flächennormalen auf den translatorischen Anteil der kommandierten Endeffektor-Zielverschiebung (Fig. 5) bezeichnet, p100 die prozentuale Realisierung der kommandierten Endeffektor-Zielverschiebung δx_d ist und |p_orth|100 die prozentuale Hindernisausweichbewe­ gung des Endeffektors ist,
• - ausgehend von den Größen, Gelenkpositionsversatz Δq:=q-q_i=0 bzw. Gelenkposition q=q_i und p=0, p_orth=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änkun­ gen erreicht sind, und dieser wird mit einer nichtnegati­ ven und möglichst nahe bei eins liegenden Zahl, die aber nicht größer als eins ist, skaliert entsprechend den in­ aktiven Nebenbedingungen, die angeben, welche Bahnbe­ schränkungen nicht erreicht sind, wobei der skalierte Newton-Optimierungsvektor auf die im vorigen Iterations­ schritt berechneten Größen, Gelenkpositionsversatz Δq und und p, p_orth, addiert wird;
• - und auf Basis der Gütefunktion und den in den neu berech­ neten Größen, Gelenkpositionsversatz Δq bzw. Gelenkposi­ tion q und p, p_orth, aktivierten Nebenbedingungen wird die Optimalität der Größen, Gelenkpositionsversatz Δq bzw. Gelenkpositionswerte q und p, p_orth, bestimmt, wobei diese Optimalität durch ein globales Minimum der Güte­ funktion gekennzeichnet ist, das allen Bahnbeschränkun­ gen, inklusive der kinematischen Gleichung, genügt.

2. Verfahren nach Anspruch 1, dadurch gekennzeichnet, daß zur Vermeidung von Kollisionen zwischen Teilen des Manipulators eben diese Teile des Manipulators selbst als Arbeitsraumhin­ dernisse definiert werden.

3. Verfahren nach Anspruch 1 oder 2, dadurch gekennzeichnet, daß die Potentialgewichtung γ_d von dem Maximalpotential, das im euklidischen Sicherheitsabstand d₀ von der durch Kolli­ sionspunkte beschriebenen Hindernisoberfläche wirkt, zum Ar­ beitsraumpotential, das im euklidischen Abstand d₁<d₀ von der durch Kollisionspunkte beschriebenen Hindernisoberfläche wirkt, in linearer Weise in Abhängigkeit vom euklidischen Ab­ stand übergeht.

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 d₀, der ebenfalls wie der Abstand d₁ vom Programmierer vorgegeben wird, keinesfalls unterschreitet, und der nicht negative Wert des Arbeitsraumpotentials wird von einem Programmierer so vorgegeben, daß sich der Manipula­ tor entsprechend der Form der Hindernisse vorkonfiguriert, wobei sich der Manipulator umso stärker vorkonfiguriert, je höher der Wert des Arbeitsraumpotentials ist.

5. Verfahren nach einem der vorhergehenden Ansprüche, dadurch gekennzeichnet, daß ein Gleiten des Endeffektors auf der Hin­ dernisoberfläche mittels einer Eigenbewegung des Endeffektors entlang des Lots der orthogonalen Projektion der mit dem Fak­ tor
skalierten Flächennormalen ∇d_f, wobei |.| die eu­ klidische Vektornorm bezeichnet, gemäß
auf die kommandierte translatorische Endeffektor-Zielver­ schiebung Δx_t ermöglicht wird, wobei ∇d_orth den Maximalbe­ trag |Δx_t|proz_orth hat und proz_orth≧0 von einem Benutzer entsprechend der maximal zulässigen Eigenbewegung vorgegeben wird, und wobei ∇d_f 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(r_k0,xm_k0) (= Kugel, die den kleinsten Abstand zu Hindernis­ sen hat, wobei bei Mehrdeutigkeit diejenige Kugel ausgewählt wird, die am nächsten zum Endeffektor liegt) liegt.

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 d₂<d₁ genügend klein ist, der eukli­ dische Abstand d₂ von einem Benutzer vorgegeben wird und die Minimalgewichtung von einem Benutzer entsprechend der erfor­ derlichen Eigenbewegung in Hindernisnähe (euklidischer Ab­ stand ≦d₁) gewählt wird und ein linearer Übergang von Maxi­ malgewichtung zu Minimalgewichtung in Abhängigkeit des eukli­ dischen Abstandes erfolgt.

7. Verfahren nach Anspruch 1, dadurch gekennzeichnet, daß sich der aktuelle Sollwert an die Gelenkregler des Manipula­ tors aus q i+1|soll:=q i|soll+Δqⁱ erklärt (mit q i|soll werden im Zeit­ raum ΔT die Gelenkpositionen durch die Gelenkregler entspre­ chend verfahren) und unter einem zulässigen Gelenkpositions­ versatz verstanden wird, daß der mit Δq aktualisierte Gelenk­ positionswert den physikalischen Gelenkanschlägen q_min, q_max entsprechend q_min≦q i|soll+Δqⁱ≦q_max, Δq den Gelenkgeschwindig­ keitsbeschränkungen _max entsprechend |Δqⁱ|≦_maxΔT genügt, die Beschränkungen der Gelenkbeschleunigungen beim eventuel­ len Auftreffen von Gelenkpositionen auf ihre physikalischen Gelenkanschläge sicher eingehalten werden und die Art der Op­ timalität von Δqⁱ von einem Operateur durch die Gütekrite­ rienwichtung α,β,γ festlegbar ist.

8. Verfahren nach Anspruch 7, dadurch gekennzeichnet, daß die Abtastzeit ΔT entsprechend ΔT≧max(Δt_q, Δt_r) so bemessen ist, daß die Rechenzeit Δt_q zur Berechnung eines optimalen zuläs­ sigen Gelenkpositionsversatzes Δq und die Einregelzeit Δt_r, welche die Regler zur Angleichung der Achspositionen an die Sollwerte q_soll benötigen, innerhalb der Zeitspanne ΔT lie­ gen.

9. Verfahren nach einem der vorhergehenden Ansprüche, gekenn­ zeichnet durch eine Anwendung bei der interaktiven, automa­ tisch kollisionsvermeidenden Bahnführung eines Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel (Spacemouse) oder dergleichen von einem Operateur kommandier­ ter Endeffektor-Zielverschiebungen in einem durch Hindernisse beschränkten Arbeitsraum.

10. Verfahren nach einem der Ansprüche 1 bis 8, gekennzeich­ net durch die Anwendung als modularer Baustein einer überge­ ordneten Aufgabe mit von der übergeordneten Aufgabe komman­ dierten Endeffektor-Zielverschiebungen in einem durch Hinder­ nisse beschränktem Arbeitsraum.

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. Verfahren nach Anspruch 10 und/oder 11, dadurch gekenn­ zeichnet, daß die zu einem Zeitpunkt T_i kommandierte kartesi­ sche Endeffektor-Zielverschiebung vom Operateur und/oder ei­ ner ü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. Verfahren nach Anspruch 1, dadurch gekennzeichnet, daß der angewandte Algorithmus der inversen Kinematik in einer Initialisierungsphase und einer nachfolgenden Optimierungs­ phase abgewickelt wird.