DE19800552A1

DE19800552A1 - Verfahren zur Kommandosteuerung eines Manipulators

Info

Publication number: DE19800552A1
Application number: DE19800552A
Authority: DE
Inventors: Maximilian Dr Schlemmer; Manfred Schedl; Michael Steinmetz; Georg Dr Gruebel; Reinhard Dr Finsterwalder
Original assignee: Deutsches Zentrum fuer Luft und Raumfahrt eV
Current assignee: Deutsches Zentrum fuer Luft und Raumfahrt eV
Priority date: 1997-06-20
Filing date: 1998-01-11
Publication date: 1999-01-28
Anticipated expiration: 2018-01-12
Also published as: DE19800552C2

Description

Die Erfindung bezieht sich auf ein Verfahren zur Komman dosteuerung eines Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel oder dgl. von einem Programmierer oder einer übergeordneten Aufgabe kommandierten Endeffektor-Ziel verschiebungen (Δx_d) in Verbindung mit einer Berechnung von Gelenkpositionswerten entsprechend einem Algorithmus in verser Kinematik.

Aus Siciliano, B., Sciavicco, L.: "Modelling and Control of Robot Manipulators", McGraw-Hill Companies (1996), S. 95 bis 101 und aus Vukobratovic, M., Kircanski, N.: "Kinematics and Trajectory Synthesis of Manipulation Robots", Springer-Verlag, Tokyo, 1986, S. 105 bis 122 sind Verfahren zur Kom mandosteuerung eines Manipulators auf der Grundlage komman dierter Endeffektor-Zielverschiebungen in Verbindung mit ei ner Berechnung entsprechend einem Algorithmus inverser Kine matik unter Benutzung der Jacobi-Matrix bekannt.

In diesem Zusammenhang ergibt sich ein Verfahren inverser Kinematik für kinematisch nichtredundante Manipulatoren, al lerdings in einem nicht-singuläre Stellungen enthaltenden Teilraum des Arbeitsbereichs. Da in der Praxis singuläre Konfigurationen auftreten, d. h. Gelenkpositionen, an denen die Jacobi-Matrix einen Rangabfall erfährt, ist dieses Ver fahren nur begrenzt einsetzbar. Wird nämlich eine singuläre Konfiguration (Singularität) angefahren, so muß der Arbeits vorgang, der mit dem Manipulator durchgeführt wird, (interaktives Bahnführen, Kraftregelung) abgebrochen oder verzögert werden.

In diesem Zusammenhang ist ein Verfahren mit Berechnung ei ner verallgemeinerten Inversen der Jacobi-Matrix bekannt, das zwar einige vorteilhafte Eigenschaften hat, dafür aber auch eine Reihe von Nachteilen mit sich bringt. So ergibt sich eine Glattheit der berechneten Gelenkbahn und ein ge ringer Verschleiß des Roboterantriebes durch Minimierung des lokalen Gelenkpositionsversatzes Δq (lokale Energiekriteri en). Außerdem wird der Abstand der Gelenkpositionen von den physikalischen Gelenkanschlägen durch Optimierung von globa len Kriterien im Nullraum der Jacobi-Matrix berücksichtigt.

Von Nachteil ist hierbei, daß Bahnbeschränkungen durch phy sikalische Gelenkanschläge nicht garantiert eingehalten wer den können, sich ein instabiles Verhalten in singulären Ro boterstellungen durch eine (verallgemeinerte) Invertierung der Jacobi-Matrix ergibt und ineffiziente Roboterbahnverläu fe möglich sind, wenn widersprechende lokale und globale Kriterien auftreten.

Bei einem anderen bekannten Verfahren, bei dem eine Berech nung der transponierten Jacobi-Matrix erfolgt, wird das kom mandierte Endeffektorziel, entsprechend einer Darstellung der Rückwärtskinematik als Optimierungsproblem, iterativ er reicht. Als vorteilhafte Eigenschaft ergibt sich hierbei ein stabiles Verhalten in singulären Roboterstellungen, da keine Invertierung der Jacobi-Matrix vorgenommen wird.

Allerdings können Bahnbeschränkungen durch physikalische Ge lenkanschläge und maximale Gelenkgeschwindigkeiten nicht ga rantiert eingehalten werden. Es müssen übergeordnete Heuri stiken zur Abdeckung dieser Anforderungen aufgestellt wer den, was sich dann in Fehlern der realen Endeffektorlage ge genüber der gewünschten Endeffektorlage niederschlägt. Es ergeben sich ineffiziente Bahnverläufe in Form von Störbewe gungen des Endeffektors, da die von der Handsteuerkugel kom mandierte kartesische Linearbewegung nicht exakt auf den Endeffektor des Manipulators übertragbar ist.

Darüber hinaus ergibt sich ein hoher Materialverschleiß des Roboterantriebes durch abruptes Durchfahren von singulären Roboterstellungen und auf Grund einer im allgemeinen ungenü gend glatten Gelenkbahn, da weder der gewichtete lokale Ge lenkpositionsversatz noch der lokale Gelenkgeschwindigkeits versatz (lokale Energie- und Beschleunigungskriterien) einer Optimierung unterzogen wurden. Außerdem muß man mit einer geringen Konvergenzgeschwindigkeit, also mit Einbußen an Echtzeitfähigkeit, auskommen, da keine praktikable optimale Strategie zur Festlegung der positiv definiten kartesischen Steifigkeitsmatrix bekannt ist.

In DE 33 44 633 C2 ist eine Echtzeitsteuerung beschrieben, bei welcher zur Berechnung der Gelenkgeschwindigkeiten red undante Gelenke, welche zur Bewegung eines Endeffektors nicht notwendig sind, festgehalten werden, und somit die Be rechnung der inversen Jacobi-Matrix vereinfacht wird. Eine derartige Berechnung wird für wenigstens eine der Ge lenk-Kombinationen durchgeführt. Die Geschwindigkeiten für jedes Gelenk werden dann durch Mittelung der berechneten Gelenkge schwindigkeiten bestimmt. Somit erfolgt bei diesem Stand der Technik eine Gewichtung, und zwar der Gelenkgeschwindigkei ten.

In US 5 430 643 ist ebenfalls ein Echtzeit-Verfahren be schrieben, bei welchem die inverse Jacobi-Matrix berechnet wird. Auch bei dem aus der vorerwähnten US-Patentschrift be kannten Verfahren findet eine Berücksichtigung von Gewichts werten für die Gelenkgeschwindigkeiten sowie von Bahnbe schränkungen zumindest für eine grafische Simulation der Ro boterbewegung statt.

Gemäß einem in der deutschen Patentanmeldung 197 03 915.4 vorgeschlagenen Verfahren ist eine interaktive Bahnführung eines kinematisch redundanten Manipulators effizient durch führbar, mit dem Vorteil einer somit weniger aufwendigen und damit benutzerfreundlichen Parametrierung. Von Nachteil ist hier lediglich, daß das Verfahren nicht für Kraftregelungs aufgaben geeignet ist, da kein einheitliches Maß existiert, das angibt, wieviel Prozent der gewünschten Endeffektor-Ziel verschiebung realisiert werden kann, und zudem die Prio rität, die Endeffektor-Zielverschiebung möglichst gut zu er füllen, verfahrensbedingt geringer ist.

Der Erfindung liegt die Aufgabe zugrunde, ein Verfahren in verser Kinematik zur Kommandosteuerung für eine interaktive Bahnführung und/oder als modularer Baustein einer übergeord neten Aufgabe (z. B. Kraftregelung) eines Manipulators unter optimiertem Beschleunigungsverhalten zu schaffen, mit dem Bahnbeschränkungen durch physikalische Gelenkanschläge sowie maximal erlaubte Gelenkgeschwindigkeiten sicher eingehalten werden, um die ermittelte Lösung auf Zulässigkeit hinsicht lich der Bahnbeschränkungen sicherzustellen, und mit dem eine minimale Belastung auf die Antriebsvorrichtung des Ma nipulators durch eine Optimierung des Beschleunigungsverhal tens der Gelenkachsen erreicht wird.

Gemäß der Erfindung, die sich auf ein Verfahren inverser Ki nematik zur Kommandosteuerung eines Manipulators der ein gangs genannten Art bezieht, wird diese Aufgabe durch die Merkmale im kennzeichnenden Teils des Anspruchs 1 gelöst. Vorteilhafte Weiterbildungen des erfindungsgemäßen Verfah rens sind Gegenstand der auf den Anspruch 1 unmittelbar oder mittelbar rückbezogenen Ansprüche 2 bis 9.

Gemäß der Erfindung, die sich auf ein Verfahren zur Komman dosteuerung eines Manipulators der eingangs angegebenen Art bezieht, ist die Aufgabe dadurch gelöst, daß

- ausgehend von einer kommandierten Endeffektor-Zielver schiebung und dem aktuellen Istwert (q_i) der Gelenkposi tionen des Manipulators wird
- unter Berücksichtigung einer zu minimierenden Gütefunkti on (f(q)), die durch nichtnegative Gewichtungswerte (α_j, β_j, γ_j) parametriert ist
- und unter Berücksichtigung von Bahnbeschränkungen durch physikalische Gelenkanschläge (q_min, q_max), maximaler Ge lenkgeschwindigkeit (_max), maximaler Gelenkbeschleuni gung (_max) in einer Umgebung physikalischer Gelenkan schläge und der kinematischen Gleichung, welche durch die Jakobi-Matrix (J(q)) repräsentiert ist

eine neue Gelenkposition (q_i+1

) berechnet, die die neuen Werte für die Gelenkregler vorgibt, wobei die Gütefunktion (f(q)) die Summe aus Energiekriterium, Referenzlagenkriteri um, Beschleunigungskriterium und einem Zusatzkriterium ist,

- dabei berechnet sich 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 ein vorgegebener Gelenkpositionswert ist, der derart festgelegt ist, daß die Folge der berech neten Gelenkpositionswerte (q_i) nahe um diesen Referenz positionswert verläuft;
- das Beschleunigungskriterium aus
(q-2q_i + q_i-1)^tdiag(γ_j)(q-2q_i + q_i-1), und
- das Zusatzkriterium aus -p,
wobei der skalare Parameter p der kinematischen Gleichung p.Δx_d = J(q_i) (q-q_i), und der Ungleichung 0 ≦ p ≦ 1 genügt, wobei p.100 die prozentuale Realisierung der kommandierten Endeffektor-Zielverschiebung (Δx_d) ist,
- ausgehend von einem Gelenkpositionswert q₁ als Startpunkt wird auf Basis der Gütefunktion ein zulässiger Optimie rungsvektor bestimmt bezüglich aller aktiven Nebenbedin gungen, 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 berech neten Gelenkposition aktivierten Nebenbedingungen wird die Optimalität dieser Gelenkpositionswerte bewertet.

Das Verfahren nach der Erfindung läßt sich bei der interak tiven Bahnführung des Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel (Spacemouse) od. dgl. von einem Operateur kommandierter Endeffektor-Zielverschiebungen an wenden.

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 einsetzen. Eine übergeordnete Aufgabe kann bei spielsweise eine solche sein, wie sie durch Kraftregelungs aufgaben gestellt ist.

Bei dem Verfahren nach der Erfindung ergibt sich ein gerin ger Verschleiß der Antriebsvorrichtung des Manipulators durch Minimierung des lokalen Gelenkpositionsversatzes Δq und des lokalen Gelenkgeschwindigkeitsversatzes (Energiekri terien und Beschleunigungskriterien). Es werden "schlanke" Gelenkbahnen um eine Referenzlage (z. B. Nullage) erzielt und dadurch wird reversibles Verhalten der Manipulatorbewegung erreicht.

Insbesondere wird dadurch vermieden, daß sich die Rollachsen des Manipulators während der Ausführung der kommandierten Manipulationsaufgabe immer stärker verdrehen und diese dann am Endanschlag zu liegen kommen und dann eine weitere Aus führung der Manipulationsaufgabe nicht mehr möglich ist.

Gelenkanschläge werden in vorteilhafter Weise weitestgehend vermieden oder sanft bzw. verschleißarm angefahren. Dieses Verhalten wird durch einen Verzögerungseffekt der Roboter achsen hervorgerufen, der proportional zum Abstand von der Referenzposition wirkt, und durch explizite Beschleunigungs beschränkungen. Darüber hinaus entsteht ein stabiles Verhal ten 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 Handsteu erkugel kommandierten kartesischen Linearbewegung auf den Endeffektor des Manipulators. Ferner werden Bahnbeschränkun gen durch physikalische Gelenkanschläge und maximale Gelenk geschwindigkeiten zuverlässig eingehalten.

Da hierdurch der Lösungsbereich, das ist die Menge der zu lässigen Gelenkpositionen, eingeschränkt wird, wird eine ho he Konvergenzgeschwindigkeit mit einem Gewinn an Echtzeitfä higkeit erreicht. Zudem wird die prozentuale Realisierung der gewünschten Endeffektor-Zielverschiebung berechnet. Sin gularitäten können in stabiler Weise angefahren und/oder durchfahren werden; daher ist auch keine Einschränkung des Arbeitsbereichs notwendig.

Eine zu einem Zeitpunkt T_i kommandierte kartesische Endef fektor-Zielverschiebung Δx_c kann vom Operateur und/oder ei ner übergeordneten Aufgabe (z. B. Kraftregelung) in Form ei nes 6-dimensionalen Inkrementvektors kommandiert werden. Al ternativ hierzu kann der Inkrementvektor durch Differenzbil dung absoluter Endeffektorkoordinaten bestimmt sein.

In den anliegenden Zeichnungen zeigen

Fig. 1 eine schematisierte Darstellung eines Ein- /Ausgabeflusses einer Rückwärtskinematik im Echt zeitraster;

Fig. 2 in Form eines Blockschaltbildes einen beim erfin dungsgemäßen Verfahren zur Kommandosteuerung eines Manipulators verwendeten Algorithmus inverser Ki nematik, und

Fig. 3 die Anwendung des erfindungsgemäßen Verfahrens zur Kommandosteuerung eines Manipulators im Rahmen der Durchführung einer übergeordneten Aufgabe, wie beispielsweise einer Kraftregelung.

In Fig. 1 ist im Zusammenhang mit den vorstehenden Merkmalen der Erfindung eine Darstellung des Ein-/Ausgabeflusses der Rückwärtskinematik im Echtzeitraster gezeigt.

Bei dem Verfahren ist unter Benutzung der Jacobi-Matrix der Kinematik zu einem Zeitpunkt T_i, eine kommandierte Endeffek tor-Zielverschiebung Δx_c: = (Δx_t, Δx_r) bezeichnet, welche vom Operateur mit Hilfe einer Spacemouse oder einer übergeordne ten Aufgabe in Form eines 6-dimensionalen Inkrementvektors kommandiert wird. Mit Δx_t ∈ R ³ bzw. Δx_r ∈ R ³ sind jeweils der translatorische bzw. rotatorische Anteil der kommandier ten Endeffektor-Zielverschiebung bezeichnet, während Δx_t ^max bzw. Δx_r ^max jeweils den maximalen (skalarwertigen), transla torischen bzw. rotatorischen kartesischen Lageversatz des Endeffektors pro Abtastzeit ΔT definieren.

Die gewünschte kartesische translatorische bzw. rotatorische Endeffektor-Zielverschiebung Δx d|t bzw. Δx d|r pro Abtastzeit ΔT ist wie folgt definiert:

Der Betrag |x| eines Vektors x ist dabei und im weiteren durch seine euklidische Norm

gegeben. Die ge wünschte Endeffektor-Zielverschiebung ist durch Δx_d: = (Δx d|t, Δx d|r) erklärt. Die Energiekriterien, Referenzla genkriterien und Beschleunigungskriterien können entspre chend den vorherrschenden problemspezifischen Anforderungen geeignet gewichtet werden, indem jeder Achse j drei positi ve, vom Operateur vorgegebene Zahlenwerte, sogenannte Ge wichtungswerte α_j, β_j, γ_j zugewiesen werden. Von den Gewich tungswerten dienen der Wert α_j der Gewichtung eines Energie kriteriums, das die Differenz zweier benachbarter berechne ter Gelenkpositionen von Achse j bewertet, der Wert β_j der Gewichtung eines Kriteriums, das die Auslenkung der Gelenk position q_j von einem ebenfalls vom Operateur vorgegebenen Referenzwert q_ref,j bewertet, und der Wert γ_j der Gewichtung eines Kriteriums, das das Beschleunigungsverhalten in Form eines Gelenkgeschwindigkeitsversatzes der Gelenkachse q_j be wertet. Der Gelenkgeschwindigkeitsversatz der Gelenkachse q_j wird genauso vom Operateur vorgegeben wie die Bahnbeschrän kungen betreffenden physikalischen Gelenkanschläge q_min, q_max des Manipulators sowie die Gelenkgeschwindigkeitsbeschrän kungen _max und die Gelenkbeschleunigungsbeschränkungen _max. Da das erfindungsgemäße Verfahren der Optimierung gewichti ger Kriterien im Echtzeittakt unter garantierter Einhaltung aller Bahnbeschränkungen dient, wird zu einer gewünschten Endeffektor-Zielverschiebung Δx i|d zum Zeitpunkt T_i-1 während des Zeitintervalls ΔT mit dem sich in einer Initialisie rungsphase und einer nachfolgenden Optimierungsphase abwic kelnden Algorithmus der inversen Kinematik ein zulässiger optimaler Gelenkpositionsversatz Δqⁱ berechnet. Hierbei ist der aktuelle Sollwert an die Gelenkregler aus q i+1|soll: = q i|soll + Δqⁱ erklärt, wobei mit q i|soll im Zeitraum ΔT die Gelenkpositionen durch die Gelenkregler entsprechend verfah ren werden. Unter einem zulässigen Gelenkpositionsversatz wird dabei verstanden, daß der mit Δq aktualisierte Gelenk positionswert den physikalischen Gelenkanschlägen q_min, q_max entsprechend q_min ≦ q_soll + Δq ≦ q_max und Δq den Gelenkge schwindigkeitsbeschränkungen _max entsprechend |Δqⁱ|≦q'_maxΔT ge nügt. Hierdurch werden die Beschränkungen der Gelenkbe schleunigungen beim Annähern von Gelenkpositionen an die physikalischen Gelenkanschläge zuverlässig eingehalten. Die Art der Optimalität von Δqⁱ ist vom Operateur durch die Gü tekriteriengewichtung α, β, γ festlegbar. Dadurch ist die Ab tastzeit ΔT dabei entsprechend ΔT ≧ max (Δt_q, Δt_r) so bemes sen, 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.

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

Es bezeichnen J_i, i = 1, . . ., ndof die Spalten der Jacobi-Ma trix der Kinematik im Punkt q_i der aktuellen Gelenkpositio nen des Manipulators, ndof die Anzahl der Gelenke und ε_i: = i|max ΔT den maximal erlaubten Gelenkpositionsversatz pro Abtastzeit ΔT. Mit

lautet die inkrementelle kinematische Gleichung:

pΔx_d = J_εy. (1)

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

Definiere weiter:

Dabei bezeichnet Δqⁱ das jüngst berechnete Inkrement der i-ten Ge lenkachse.

Damit alle Bahnbeschränkungen erfüllt werden können, ist, gemäß der kinematischen Gleichung (1), die gewünschte Endeffektor- Zielverschiebung einer zentrischen Streckung unter worfen, die in Form eines Skalars beschrieben ist:

0 ≦ p ≦ 1.

Die Größe p wird durch das erfindungsgemäße Verfahren so maximiert, daß alle Bahnbeschränkungen eingehalten werden. Dabei ist p.100 die prozentuale Realisierung der gewünschten Endeffektor-Zielverschiebung.

Definiere den Begrenzungsvektor
b: = (0, 0, 0, 0, 0, 0, y_min, 0,-y_max,-1) ∈ R ²ⁿ⁺⁶, den zu bestimmenden Parametervektor x: = (y, p) und die Matrix der Gradienten aller Nebenbedingungen A ∈ R ^2n+6,n mit n: = ndof+1:

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

Die Gewichte α, β, γ der Kriterien definieren den Cholesky- Faktor der Hessenmatrix aus der skalierten Summe der Kriterien in Form der Diagonalmatrix Λ ∈ R ^n,n. Diese Kriterien sind das Energiekriterium (q-q_i) ^tdiag(α_j)(q-q_i), das Refe renzlagenkriterium (q-q_ref)^tdiag(β_j)(q-q_ref), wobei die Größe q_ref ein vorgegebener Gelenkpositionswert ist, der derart festgelegt ist, daß die Folge der berechneten Gelenkpositions werte (q_i) nahe um diesen Referenzpositionswert verläuft; das Beschleunigungskriterium (q - 2q_i + q_i-1)^tdiag(γ_j)(q - 2q_i + q_i-1) und das Zusatzkriterium aus -p, wobei der skalare Parameter p der kinematischen Gleichung pΔx_d=J(q_i) (q-q_i) und der Un gleichung 0 ≦ p ≦ 1 genügt und p.100 die prozentuale Realisierung der kommandierten Endeffektor-Zielverschiebung (Δx_d) ist. Die Hauptdiagonalelemente der vorerwähnten Diagonalmatrix lauten:

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

Anhand der Fig. 2 wird im folgenden ein beim Verfahren nach der Erfindung benutzter Algorithmus für die inverse Kinematik genauer beschrieben. Beim verwendeten Algorithmus der inversen Kinematik wird zuerst eine Initialisierungsphase durchgeführt, die folgendermaßen ausgestaltet ist:
Zählindex: k = 0; Startwert: x_k = (y_k, p_k) = 0; k max < 2; 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 Kenn zeichnung der aktiven, singulären und inaktiven Nebenbedingungen:

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 Gütefunktion f (q)

2(β scal|i(y i|k - y i|ref) + γ scal|i(y i|k - y i|acc) + α scal|iy i|k, -1),

daß beim verwendeten Algorithmus der inversen Kinematik anschließend eine Optimierungsphase durchgeführt wird, die folgendermaßen ausgestaltet ist:

I. Berechne die 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. Definiere die Optimierungsrichtung:

d_k: = Z_kd_Z.

II. Bestimme die maximale Schrittweite s_k und den Index j i0|k der beschränkenden Nebenbedingung:

(Bei Mehrdeutigkeit wähle dasjenige j0 mit kleinstem Index i0).

III. Prüfe auf Optimalität und Update aller Matrix- und Index größen.

(a) Falls s_k < 1 (Nebenbedingung j 0|k ist aktiv geworden):
- i. Falls m_k < n (es gibt noch inaktive Nebenbedingungen):
- - x_k+1 = x_k + s_kd_k, x_k+1 = (y_k+1, p_k+1);
  g_k+1 = 2(β scal|i(y i|k+1 - y i|ref) + γ scal|i(y i|k+1 - y i|acc) + α scal|iy i|k+1, -1).
- - Aktualisiere Matrixfaktorisierungen und Indexmenge:
  (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 IV.
  Sonst: k = k + 1 (erhöhe Iterationsindex)
  Falls k < k max: Gehe zu Schritt IV.
  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 I.
- ii. Falls m_k = n (Ecke des zulässigen Bereichs erreicht, Austausch aktiver Nebenbedinungen erforderlich):
- - x_k+1 = x_k + s_kd_k, x_k+1 = (y_k+1, p_k+1);
  g_k+1 = 2(β scal|i(y i|k+1 - y i|ref) + γ scal|i(y i|k+1 - y i|acc) + α scal|iy_k+1,-1);
  λ_k = LLS(r_k+1, m_k+1, g_k, Y_k, L_k).
- - Gib eine Richtung frei mit höchstem Optimierungspotential:
- - Aktualisiere Matrixfaktorisierungen und Indexmenge:
  (_k, _k, _k, _k, _k, _k, _k): = Φ A|-(j0, r_k, m_k, J_k, Y_k, Z_k, L_k, S_k).
- - Aktiviere Nebenbedingungen j i0|k.
  Aktualisiere Matrixfaktorisierungen und Indexmenge:
  (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 IV.
  Sonst: k = k+1 (erhöhe Iterationsindex)
  Falls k < k max: Gehe zu Schritt IV.
  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 I.
(b) Falls s_k = 1 (keine neue aktive Nebenbedingung).
- x_k+1 = x_k + d_k, x_k+1 = (y_k+1, p_k+1);
g_k+1 = 2(β scal|i(y i|k+1 - y i|ref) + γ scal|i(y i|k+1 - y i|acc) + α scal|iy i|k+1, -1);
- Aktualisiere Matrixfaktorisierungen und Indexmenge:
(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 IV.
Sonst: Gib eine Richtung frei mit höchstem Optimierungspotential:
- Aktualisiere Matrixfaktorisierungen und Indexmenge:
(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 IV.
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 I.

IV. Lösung x_k+1 ermittelt. Stop!
Ende des Algorithmus der inversen Kinematik.
Definition der Funktion LLS:

λ = LLS(r, m, g, Y, L).
λ_i: = 0, i = r + 1, . . . , m;
λ_hL: = (λ_r+1 , . . . , λ_m);
L^tλ_L = Y^tg.

Bestimme hieraus λ_L∈R ^r durch Rückwärtssubstitution. Definiere den Ausgabewert der Funktion:

λ: = (λ_L, λ_hL).

Mit l_eq: = 1 + max_(1≦i≦m) |λ(i)| setze diejenigen Komponenten von λ auf den Wert l_eq, die zu den Gleichungsnebenbedingungen (1) gehören.
Ende der Funktion LLS.
Definition der Funktion Φ₊:

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

Aktivere Nebenbedingung jⁱ⁰. Es bezeichne a_j0 mit j0: = jⁱ⁰ die jⁱ⁰ Zeile von A. Definiere Q^t: = (Y, Z) und bilde a: = Qa_j0. Partitioniere a =: (a_Y, a_Z) mit a_Y ∈ R und a_Z ∈ R ^n-r. Bestimme die Householderreflexion ∈ R ^n-r,n-r so, daß gilt:

|a_Z|e₁ = a_Z,

mit e₁: = (1,0,. . ,0) ∈ R ^n-r. Definiere den unitären Transformator H ∈ R ^n,n gemäß:

Definiere den unitären Transformator:

Q: = HQ;

Aktualisiere Indizies:

Vertausche Nebenbedingungen:
Falls r ≠ r, dann

Aktualisiere Matrixfaktorisierungen:
Y ist die Teilmatrix von Q^t, die die Spalten 1 bis r von Q^t enthält.
Z ist die Teilmatrix von Q^t, die die Spalten r + 1 bis n von Q^t enthält.

Ende der Funktion Φ₊.

Definition der Funktion Φ_-:

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

Inaktiviere Nebenbedingung j^j0.

- Falls j0 < r: (Eliminiere singuläre aktive Nebenbedingung) Aktualisiere Indizies:
r: = r;
m: = m-1;
Vertausche Nebenbedingungen:
Aktualisiere Matrixfaktorisierungen:
Y: = Y;
Z: = Z;
L: = L;
S: = (S₁, . . , S_j0-r-1, S_j0-r+1, . . , S_m-r),
dabei bezeichnen S_i die Spalten von S.
- Falls j0 ≦ r: (Eliminiere reguläre aktive Nebenbedingung)
ergibt sich aus R: = L^t durch Streichen der j0-ten Spalte. Die an den Stellen (j0, j0 + 1), (j0 + 1, j0 + 2), . . , (r, r + 1) auftretenden Elemente von werden durch Linksmultiplikation mit einer Folge von unitären Eliminationsmatrizen _{j0, j0+1}, . . , _r-1,r ∈ R ^r,r annulliert. Definiere lineare Trans formatoren:
- Falls die letzte Zeile von null ist oder falls r = m:
Aktualisiere Indizies:
r: = r-1;
m: m-1;
Vertausche Nebenbedingungen:
Aktualisiere Matrixfaktorisierungen:
Y ist die Teilmatrix von Q^t, die die Spalten 1 bis r von Q^t enthält.
Z ist die Teilmatrix von Q^t, die die Spalten r+1 bis n von Q^t enthält. Falls r < m streiche die letzte Zeile von und . Setze
L: = ^t;
S: = ;
- Sonst (wandle singuläre aktive Nebenbedingung in reguläre aktive Nebenbedingung):
Bestimme das Element der letzten Zeile von mit dem kleinsten Index l0 derart, daß gilt:
_r,10 ≠ 0.
Vertausche Spalte l0 mit Spalte 1 von . Definiere linearen Transformator:
R: = (, _l0),
dabei bezeichnet _l0 die l0-te Spalte von .
Aktualisiere Indizies:
r: = r;
m: = m -1;
Vertausche Nebenbedingungen:
Aktualisiere Matrixfaktorisierungen:
Y ist die Teilmatrix von Q^t, die die Spalten 1 bis r von Q^t enthält.
Z ist die Teilmatrix von Q^t, die die Spalten r+1 bis n von Q^t enthält.
L: = R^t;
S: = (₂, . . ., _10-1, ₁, ₁₀₊₁, . . ., _m-r),
dabei bezeichnen die ₁ die Spalten von .
Ende der Funktion Φ_-.
Definition der Funktion Φ A|0:
(r, J, Y, Z, L, S): = Φ A|0(Â, m, J).
1. Definiere:
A⁰: = Â, i: = 0.
2. Definiere folgende Matrizenrekursion:
Aⁱ⁺¹: = H_iAⁱ, i ≧ 0.
Der unitäre Transformator H_i ∈ R ^n,n ist wie folgt erklärt:
Die Householderreflexion _i ∈ R ^{n-i, n-i} ist so definiert, daß gilt:
|ã i|k0|e₁= _iã i|k0,
mit e₁: = (1, 0, . . ,0) ∈ R ^n-i. Dabei bezeichnen die Vektoren ã i|k ∈ R ^n-i, k = 1, . . , n-i die Spalten der Matrix Ãⁱ ∈ R ^{n-i, m-i}:
Ferner ist 1 ≦ k0 ≦ n-i der kleinste Index mit ã i|k0 ≠ 0. Falls kein solches k0 existiert, dann gehe zu Schritt III, sonst gehe zu Schritt IV.
3. Definiere Indizies:
r: = i;
J: = (jⁱ, . . , j²ⁿ⁺⁶).
Definiere Matrixfaktorisierungen:
Q: = H_-1H_-2 . . . H₀;
Y ist die Teilmatrix von Q^t, die die Spalten 1 bis r von Q^t enthält.
Z ist die Teilmatrix von Q^t, die die Spalten r + 1 bis n von Q^t enthält.
Es ist T die Matrix, die durch Streichen der Zeilen r + bis n aus Aⁱ⁺¹ entsteht.
Definiere L ∈ R und S ∈ R ^{, m-} gemäß:
(L^t, S): = T.
Stop: Ausführung der Funktion Φ A|0 ist beendet!
4. Vertausche die Spalte i + k0 mit der Spalte i + 1 in Aⁱ.
Setze j^k: = j^k k, erhöhe den Zählindex i = i + 1 und gehe zu Schritt II.
Ende der Funktion Φ A|0.
Definition der Funktion Φ Z|0:
(Q^Z, R^Z): = Φ Z|0(Z, r).
Bestimme die QR-Dreieckszerlegung von Z:
Dabei bezeichnen Q^Z ∈ R ^n,n eine unitäre Matrix und R^Z ∈ R ^{r, r} eine obere Dreiecksmatrix.
Ende der Funktion Φ Z|0.

Claims

1. Verfahren inverser Kinematik zur Kommandosteuerung ei nes Manipulators auf der Basis von mit Hilfe einer Handsteu erkugel oder dgl. von einem Programmierer oder einer überge ordneten Aufgabe kommandierten Endeffektor-Zielverschie bungen (Δx_d) in Verbindung mit einer Berechnung von Gelenk positionswerten entsprechend einem Algorithmus inverser Ki nematik, gekennzeichnet durch folgende Merkmale:

- ausgehend von einer kommandierten Endeffektor-Zielver schiebung und dem aktuellen Istwert (q_i) der Gelenkposi tionen des Manipulators wird
- unter Berücksichtigung einer zu minimierenden Gütefunk tion (f(q)), die durch nichtnegative Gewichtungswerte (α_j, β_j, γ_j) parametriert ist
- und unter Berücksichtigung von Bahnbeschränkungen durch physikalische Gelenkanschläge (q_min, q_max), maximaler Ge lenkgeschwindigkeit (_max), maximaler Gelenkbeschleuni gung (_max) in einer Umgebung physikalischer Gelenkan schläge und der kinematischen Gleichung, welche durch die Jakobi-Matrix (J(q)) repräsentiert ist
eine neue Gelenkposition (q_i+1) berechnet, die die neuen Werte für die Gelenkregler vorgibt, wobei die Gütefunktion (f(q)) die Summe aus Energiekriterium, Referenzlagenkriteri um, Beschleunigungskriterium und einem Zusatzkriterium ist,
- dabei berechnet sich 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 ein vorgegebener Gelenkpositionswert ist, der derart festgelegt ist, daß die Folge der be rechneten Gelenkpositionswerte (q_i) nahe um diesen Refe renzpositionswert verläuft;
- das Beschleunigungskriterium aus
(q - 2q_i + q_i-1)^tdiag(γ_j)(q - 2q_i + q_i-1), und
- das Zusatzkriterium aus -p,
wobei der skalare Parameter p der kinematischen Glei chung p.Δx_d = J(q_i)(q-q_i), und der Ungleichung 0 ≦ p ≦ 1 genügt, wobei p.100 die prozentuale Realisierung der kommandierten Endeffektor-Zielverschiebung (Δx_d) ist,
- ausgehend von einem Gelenkpositionswert q_i als Start punkt wird auf Basis der Gütefunktion ein zulässiger Op timierungsvektor bestimmt bezüglich aller aktiven Neben bedingungen, die angeben, welche Bahnbeschränkungen er reicht sind und dieser wird skaliert entsprechend den inaktiven Nebenbedingungen, die angeben, welche Bahnbe schränkungen nicht erreicht sind;
- der skalierte Optimierungsvektor wird auf den im vorigen Iterationsschritt berechneten Gelenkpositionswert ad diert;
- und auf Basis der Gütefunktion und der in der neu be rechneten Gelenkposition aktivierten Nebenbedingungen wird die Optimalität dieser Gelenkpositionswerte bewer tet.

2. Verfahren nach Anspruch 1 unter Benutzung der Jacobi-Matrix der Kinematik, wobei ausgehend von einer kommandier ten Endeffektor-Zielverschiebung Δx_c: = (Δx_t, Δx_r) zu einem Zeitpunkt T_i, worin Δx_t ∈ R ³ bzw. Δx_r ∈ R ³ jeweils den translatorischen bzw. rotatorischen Anteil der kommandierten Endeffektor-Zielverschiebung bezeichnen, Δx_t ^max bzw. Δx_r ^max jeweils den maximalen (skalarwertigen), vom Operateur vorge gebenen translatorischen bzw. rotatorischen kartesischen La geversatz des Endeffektors pro Abtastzeit ΔT definieren, die gewünschte kartesische translatorische bzw. rotatorische Endeffektor-Zielverschiebung Δx d|t bzw. Δx d|r pro Abtastzeit ΔT wie folgt erklärt ist:
(der Betrag |x| eines Vektors x ist dabei und im weiteren durch seine euklidische Norm
gegeben), und die gewünschte Endeffektor-Zielverschiebung durch Δx_d: = (Δx d|t, Δx d|r) erklärt ist, dadurch gekennzeich net, daß die drei Kriterien, nämlich die Energiekriterien, Referenzlagenkriterien und Beschleunigungskriterien entspre chend den vorherrschenden problemspezifischen Anforderungen geeignet gewichtet werden, indem jeder Achse j drei positi ve, vom Operateur vorgegebene Zahlenwerte α_j, β_j, γ_j zugewie sen werden, von denen der Wert der Gewichtung eines Ener giekriteriums, das die Differenz zweier benachbarter berech neter Gelenkpositionen von Achse j bewertet, der Wert β_j der Gewichtung eines Kriteriums, das die Auslenkung der Ge lenkposition q_j von einem ebenfalls vom Operateur vorgege benen Referenzwert q_ref,j bewertet, und der Wert γ_j der Ge wichtung eines Kriteriums, das das Beschleunigungsverhalten in Form eines Gelenkgeschwindigkeitsversatzes der Gelenk achse q_j bewertet, dient, der genauso vom Operateur vorgege ben wird wie die Bahnbeschränkungen betreffenden physikali schen Gelenkanschläge q_min, q_max des Manipulators sowie die Gelenkgeschwindigkeitsbeschränkungen _max und die Gelenkbe schleunigungsbeschränkungen _max, daß bei der Erlangung der Verfahrensausgangsgrößen zu einer gewünschten Endeffektor-Ziel verschiebung Δx i|d zum Zeitpunkt T_i-1 während des Zeitin tervalls ΔT mit dem sich in einer Initialisierungsphase und einer nachfolgenden Optimierungsphase abwickelnden Algorith mus der inversen Kinematik ein zulässiger optimaler Gelenk positionsversatz Δqⁱ berechnet wird, wobei sich der aktuelle Sollwert an die Gelenkregler aus q i+1|soll: = q i|soll + Δqⁱ erklärt (mit q i|soll werden im Zeitraum ΔT die Gelenkpositionen durch die Gelenkregler entsprechend verfahren) und unter einem zu lässigen Gelenkpositionsversatz verstanden wird, daß der mit Δq aktualisierte Gelenkpositionswert den physikalischen Ge lenkanschlägen q_min, q_max entsprechend q_min ≦ q_soll + Δq ≦ q_max und Δq den Gelenkgeschwindigkeitsbeschränkungen _max ent sprechend |Δqⁱ| ≦ q'_maxΔT genügt und die Beschränkungen der Ge lenkbeschleunigungen beim Annähern von Gelenkpositionen auf die physikalischen Gelenkanschläge zuverlässig eingehalten werden und die Art der Optimalität von Δqⁱ vom Operateur durch die Gütekriteriengewichtung α, β, γ festlegbar ist und die Abtastzeit ΔT dabei entsprechend ΔT ≧ max (Δt_q, Δt_r) so bemessen ist, daß die Rechenzeit Δt_q zur Berechnung eines optimalen zulässigen Gelenkpositionsversatzes Δq und die Einregelzeit Δt_r welche die Regler zur Angleichung der Achs positionen an die Sollwerte q_soll benötigen, innerhalb der Zeitspanne ΔT liegen.

3. Verfahren nach Anspruch 2, gekennzeichnet durch die Anwendung bei der interaktiven Bahnführung des Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel (Spacemouse) od. dgl. von einem Operateur kommandierter End effektor-Zielverschiebungen.

4. Verfahren nach Anspruch 2, gekennzeichnet zur An wendung als modularer Baustein einer übergeordneten Aufgabe mit von der übergeordneten Aufgabe kommandierten Endeffek tor-Zielverschiebungen.

5. Verfahren nach Anspruch 4, dadurch gekenn zeichnet, daß die übergeordnete Aufgabe eine solche ist, wie sie durch Kraftregelungsaufgaben gestellt ist.

6. Verfahren nach Anspruch 2, dadurch gekenn zeichnet, daß die inkrementelle kinematische Gleichung unter der Voraussetzung, daß J_i, i = 1, . . ., ndof die Spalten der Jacobi-Matrix der Kinematik im Punkt q_i, ndof die Anzahl der Gelenke und ε_i: = i|maxΔT den maximal erlaubten Ge lenkpositionsversatz pro Abtastzeit ΔT beschreiben, mit
pΔx_d = J_εy (1)
lautet, daß sich aufgrund der Bahnbeschränkungen die Boxbe schränkungen
ergeben, wobei q_i den jüngst berechneten, zulässigen Sollpo sitionssatz, auf dessen Werte die Achsen des Robotors bereits eingeregelt sind, bezeichnet, daß weiterhin
wobei Δq¹ das jüngst berechnete Inkrement der i-ten Gelenkachse bezeichnet, definiert wird, daß zur Erfüllung aller Bahnbeschränkungen gemäß der kinematischen Gleichung (1) die gewünschte Endeffektor- Zielverschiebung einer zentrischen Streckung unterworfen wird, die sich in Form eines Skalars 0 ≦ p ≦ 1 beschreiben läßt, wobei p.100 die prozentuale Realisierung der gewünschten Endeffektor-Zielverschiebung ist und die Größe p so maximiert wird, daß alle Bahnbeschränkungen eingehalten werden, daß b: = (0,0,0,0,0,0,y_min,0,-y_max,-1) ∈ R ²ⁿ⁺⁶, den zu be stimmenden Parametervektor x: = (y, p) und die Matrix der Gradienten aller Nebenbedingungen A ∈R ^2n+6,n mit n: = ndof + 1:
definiert werden, wobei I_n ∈ R ^n,n die Einheitsmatrix und a_i, i = 1, . . ., 2n + 6 die Zeilen von A bezeichnen, und daß die Gewichte α, β, γ der Kriterien den Cholesky-Faktor der Hesse matrix aus der skalierten Summe der Kriterien, nämlich des Energiekriteriums, des Referenzlagenkriteriums, des Be schleunigungskriteriums und des Zusatzkriteriums, in Form der Diagonalmatrix Λ ∈ R ^n,n definieren, deren Hauptdiagonal elemente
lauten.

7. Verfahren nach Anspruch 6, dadurch gekennzeichnet, daß beim verwendeten Algotrithmus der inversen Kinematik eine Initialisierungsphase durchgeführt wird, die folgendermaßen ausgestaltet ist:
Zählindex: k = 0; Startwert: x_k = (y_k, p_k) = 0; k max < 2; 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 Kenn zeichnung der aktiven, singulären und inaktiven Nebenbedingungen:
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 Gütefunktion f(q):
g₀ ∈ R ⁿ: g_k: = 2(β scal|1 (y 1|k - y 1|ref) + γ scal|1 (y 1|k - y 1|acc) + α scal|1y 1|k, . . ., β scal|ndof (y ndof|k - y ndof|ref) + γ scal|ndof(y ndof|k - y ndof|acc) + α scal|ndofy ndof|k, -1),
wobei für g_k folgende Kurzschreibweise verwendet wird:
2(β scal|i(y i|k - y i|ref) + γ scal|i(y i|k - y i|acc) + α scal|iy i|k, -1),
daß beim verwendeten Algorithmus der inversen Kinematik anschließend eine Optimierungsphase durchgeführt wird, die folgendermaßen ausgestaltet ist:
I. Berechne die 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. Definiere die Optimierungsrichtung:
d_k: = Z_kd_Z.
II. Bestimme die maximale Schrittweite s_k und den Index j i0|k der beschränkenden Nebenbedingung:
(Bei Mehrdeutigkeit wähle dasjenige j0 mit kleinstem Index i0).
III. Prüfe auf Optimalität und Update aller Matrix- und Index größen.

(a) Falls s_k < 1 (Nebenbedingung j 0|k ist aktiv geworden):
- i. Falls m_k < n (es gibt noch inaktive Nebenbedingungen):
- - x_k+1 = x_k + s_kd_k, x_k+1 = (y_k+1, p_k+1);
  g_k+1 = 2(β scal|i(y i|k+1 - y i|ref) + γ scal|i(y i|k+1 - y i|acc) + α scal|iy i|k+1,-1).
- - Aktualisiere Matrixfaktorisierungen und Indexmenge:
  (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 IV.
  Sonst: k = k + 1 (erhöhe Iterationsindex)
  Falls k < k max: Gehe zu Schritt IV.
  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 I.
- ii. Falls m_k = n (Ecke des zulässigen Bereichs erreicht, Austausch aktiver Nebenbedingungen erforderlich):
- - x_k+1 = x_k + s_kd_k, x_k+1 = (y_k+1, p_k+1);
  g_k+1 = 2(β scal|i(y i|k+1 - y i|ref) + γ scal|i(y i|k+1 - y i|acc) + α scal|iy_k+1,-1);
  λ_k = LLS(r_k+1, m_k+1, g_k, Y_k, L_k).
- - Gebe eine Richtung frei mit höchstem Optimierungspotential:
- - Aktualisiere Matrixfaktorisierungen und Indexmenge:
  (_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 Indexmenge:
  (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 IV.
  Sonst: k = k+1 (erhöhe Iterationsindex)
  Falls k < k max: Gehe zu Schritt IV.
  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 I.
- (b) Falls s_k = 1 (keine neue aktive Nebenbedingung).
- - x_k+1 = x_k + d_k, x_k+1 = (y_k+1, p_k+1);
  g_k+1 = 2(β scal|i(y i|k+1 - y i|ref) + γ scal|i(y i|k+1 - y i|acc) + α scal|iy i|k+1,-1);
- - Aktualisiere Matrixfaktorisierungen und Indexmenge:
  (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 IV.
  Sonst: Gib eine Richtung frei mit höchstem Optimierungspotential:
- - Aktualisiere Matrixfaktorisierungen und Indexmenge:
  (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 IV.
  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 I.
IV. Lösung x_k+1 ermittelt. Stop!
Ende des Algorithmus der inversen Kinematik.
Definition der Funktion LLS:
λ = LLS(r, m, g, Y, L).
λ_i: = 0, i = r + 1, . . ., m;
λ_hL: = (λ_r+1 , . . ., λ_m);
L^tλ_L = Y^tg.
Bestimme hieraus λ_L∈R ^r durch Rückwärtssubstitution. Definiere den Ausgabewert der Funktion:
λ: = (λ_L, λ_hL).
Mit l_eq: = 1 + max_(1≦i≦m) |λ(i)| setze diejenigen Komponenten von λ auf den Wert l_eq, die zu den Gleichungsnebenbedingungen (1) gehören.
Ende der Funktion LLS.
Definition der Funktion Φ₊:
(r, m, J, Y, Z, L, S) = Φ A|+(i0, r, m, J, Y, Z, L, S).
Aktiviere Nebenbedingung jⁱ⁰. Es bezeichne a_j0 mit j0: = jⁱ⁰ die jⁱ⁰ Zeile von A. Definiere Q^t: = (Y, Z) und bilde a: = Qa_j0.
Partitioniere a =: (a_Y, a_Z) mit a_Y ∈ R ^r und a_Z ∈R ^n-r. Bestimme die Householderreflexion ∈ R ^{n-r, n-r} so, daß gilt:
|a^Z|e₁ = a_Z,
mit e₁: = (1,0, . . ,0) ∈ R ^n-r. Definiere den unitären Transformator H ∈ R ^n,n gemäß
Definiere den unitären Transformator:
Q: = HQ;
Aktualisiere Indizies:
Vertausche Nebenbedingungen:
Falls r ≠ r, dann
Aktualisiere Matrixfaktorisierungen:
Y ist die Teilmatrix von Q^t, die die Spalten 1 bis r von Q^t enthält.
Z ist die Teilmatrix von Q^t, die die Spalten r + 1 bis n von Q^t enthält.
Ende der Funktion Φ₊.
Definition der Funktion Φ_-:
(r, m, J, Y, Z, L, S) = Φ A|-(j0, r, m, J, Y, Z, L, S).
Inaktiviere Nebenbedingung j^j0.
- Falls j0 < r: (Eliminiere singuläre aktive Nebenbedingung)
Aktualisiere Indizies:
r: = r;
m: = m-1;
Vertausche Nebenbedingungen: Aktualisiere Matrixfaktorisierungen:
Y: = Y;
Z: = Z;
L: = L;
S: = (S₁, . . , S_j0-r-1, S_j0-r+1, S_m-r),
dabei bezeichnen S_i die Spalten von S.
- Falls j0 ≦ r: (Eliminiere reguläre aktive Nebenbedingung)
ergibt sich aus R: = L^t durch Streichen der j0-ten Spalte. Die an den Stellen (j0, j0 + 1), (j0 + 1, j0 + 2), . . , (r, r + 1) auftretenden Elemente von werden durch Linksmultiplikation mit einer Folge von unitären Eliminationsmatrizen _{j0, j0+1}, . . , _{r-1, r} ∈ R ^r,r annulliert. Definiere lineare Transformatoren:
- Falls die letzte Zeile von null ist oder falls r = m:
Aktualisiere Indizies:
r: = r - 1;
m: = m - 1;
Vertausche Nebenbedingungen:
Aktualisiere Matrixfaktorisierungen:
Y ist die Teilmatrix von Q^t, die die Spalten 1 bis r von Q^t enthält.
Z ist die Teilmatrix von Q^t, die die Spalten r + 1 bis n von Q^t enthält. Falls r < m streiche die letzte Zeile von und . Setze:
L: = ^t;
S: = ;
- Sonst (wandle singuläre aktive Nebenbedingung in reguläre aktive Nebenbedingung):
Bestimme das Element der letzten Zeile von mit dem kleinsten Index l0 derart, daß gilt:
_r,l0 ≠ 0.
Vertausche Spalte l0 mit Spalte 1 von . Definiere linearen Transformator:
R: = (, _l0),
dabei bezeichnet _l0 die l0-te Spalte von .
Aktualisiere Indizies:
r: = r;
m: = m - 1;

Vertausche Nebenbedingungen:
Aktualisiere Matrixfaktorisierungen:
Y ist die Teilmatrix von Q^t, die die Spalten 1 bis r von Q^t enthält.
Z ist die Teilmatrix von Q^t, die die Spalten r + 1 bis n von Q^t enthält.
L: = R^t;
S: = (₂, . . .,_l0-1, ₁, _l0+1, . . ., _m-r),
dabei bezeichnen die ₁ die Spalten von .
Ende der Funktion Φ_-.
Definition der Funktion Φ A|0:
(r, J, Y, Z, L, S): = Φ A|0(Â, m, J).
- 1. Definiere:
  A⁰: = Â, i: = 0.
- 2. Definiere folgende Matrizenrekursion:
  Aⁱ⁺¹: = H_iAⁱ, i ≧ 0.
  Der unitäre Transformator H_i ∈ R ^{n, n} ist wie folgt erklärt:
  Die Householderreflexion _i ∈ R ^{n-i, n-i} ist so definiert, daß gilt:
  |ã i|k0|e₁ = _iã i|k0,
  mit e₁: = (1,0, . . .,0) ∈ R ^n-i. Dabei bezeichnen die Vektoren ã i|k ∈ R ^n-i, k = 1, . . ., n - i die Spalten der Matrix Ãⁱ ∈ R ^{n-i, m-i}:
  Ferner ist ≦ k0 ≦ n-i der kleinste Index mit ã i|k0 ≠ 0. Falls kein solches k0 existiert, dann gehe zu Schritt III, sonst gehe zu Schritt IV.
- 3. Definiere Indizies:
  r: = i;
  J: = (jⁱ, . ., j²ⁿ⁺⁶).
  Definiere Matrixfaktorisierungen:
  Q: = H_-1H_-2 . . . H₀.
  Y ist die Teilmatrix von Q^t, die die Spalten 1 bis r von Q^t enthält.
  Z ist die Teilmatrix von Q^t, die die Spalten r + 1 bis n von Q^t enthält.
  Es ist T die Matrix, die durch Streichen der Zeilen r + 1 bis n aus Aⁱ⁺¹ entsteht.
  Definiere L ∈ R und S ∈ R ^{, m-} gemäß:
  (L^t, S): = T.
  Stop: Ausführung der Funktion Φ A|0 ist beendet!
- 4. Vertausche die Spalte i + k0 mit der Spalte i + 1 in Aⁱ.
  Setze j^k: = j^k k, erhöhe den Zählindex i = i + 1 und gehe zu Schritt II.
  Ende der Funktion Φ A|0.
  Definition der Funktion Φ Z|0:
  (Q^Z, R^Z): = Φ Z|0(Z, r).
  Bestimme die QR-Dreieckszerlegung von Z:
  Dabei bezeichnen Q^Z ∈ R ^n,n eine unitäre Matrix und R^Z ∈ R ^r,r eine obere Dreiecksmatrix.
  Ende der Funktion Φ Z|0.

8. Verfahren nach einem der Ansprüche 2 bis 5, dadurch gekennzeichnet, daß die zu einem Zeitpunkt T_i kommandierte kartesische Endeffektor-Zielverschiebung Δx_c vom Operateur und/oder einer übergeordneten Aufgabe, z. B. einer Kraftregelungsaufgabe, in Form eines 6-dimensionalen Inkre mentvektors kommandiert wird.

9. Verfahren nach einem der Ansprüche 2 bis 5, dadurch gekennzeichnet, daß der Inkrementvektor der zu einem Zeitpunkt T_i kommandierten kartesischen Endeffektor-Zielver schiebung Δx_c vom Operateur und/oder einer übergeordneten Aufgabe, z. B. einer Kraftregelungsaufgabe, durch Differenz bildung absoluter Endeffektorkoordinaten bestimmt wird.