DE19703915C1

DE19703915C1 - Verfahren zur Kommandosteuerung für eine interaktive Bahnführung eines kinematisch redundanten Manipulators

Info

Publication number: DE19703915C1
Application number: DE19703915A
Authority: DE
Inventors: Maximilian Schlemmer
Original assignee: Deutsches Zentrum fuer Luft und Raumfahrt eV
Current assignee: Deutsches Zentrum fuer Luft und Raumfahrt eV
Priority date: 1997-02-03
Filing date: 1997-02-03
Publication date: 1998-08-06
Anticipated expiration: 2017-02-04
Also published as: US6081754A; FR2761292A1; FR2761292B1

Description

Die Erfindung bezieht sich auf ein Verfahren zur Kommando steuerung für eine interaktive Bahnführung eines kinema tisch redundanten Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel od. dgl. von einem Programmierer kom mandierten Endeffektor-Zielverschiebungen in Verbindung mit einer Berechnung entsprechend einem Algorithmus inverser Ki nematik unter Benutzung der Jacobi-Matrix der 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 für eine Bahnführung eines kinematisch redun danten Manipulators auf der Grundlage von einem Programmie rer kommandierter Endeffektor-Zielverschiebungen in Verbin dung mit einer Berechnung entsprechend einem Algorithmus in verser Kinematik unter Benutzung der Jacobi-Matrix bekannt.

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 (lokale Energiekriterien). Außerdem wird der Abstand der Gelenkpositionen von den phy sikalischen Gelenkanschlägen durch Optimierung von globalen 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.

Es erfolgt allerdings keine Berücksichtigung von Bahnbe schränkungen durch physikalische Gelenkanschläge und maxima le Gelenkgeschwindigkeiten. Es müssen übergeordnete Heuri stiken zur Abdeckung dieser Anforderungen aufgestellt wer den. Es ergeben sich ineffiziente Bahnverläufe in Form von Störbewegungen des Endeffektors, da die von der Handsteuer kugel kommandierte 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 (keine Berücksichtigung lokaler Energiekriterien). Außerdem muß man mit einer geringen Kon vergenzgeschwindigkeit, also mit Einbußen an Echtzeitfähig keit, auskommen, da keine praktikable optimale Strategie zur Festlegung der positiv definiten kartesischen Steifigkeits matrix bekannt ist.

In DE 33 44 633 C2 ist eine Echtzeitsteuerung beschrieben, bei welcher zur Berechnung der Gelenkgeschwindigkeit der redundanten 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 Gelenk- Kombinationen durchgeführt. Die Geschwindigkeiten für jedes Gelenk werden dann durch Mittelung der berechneten Gelenkge schwindigkeiten bestimmt. Somit erfolgt beim Stand der Tech nik eine Gewichtung, und zwar der Gelenkgeschwindigkeiten.

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 US-Patentschrift bekannten Ver fahren findet eine Berücksichtigung von Gewichtswerten für die Gelenkgeschwindigkeiten sowie von Bahnbeschränkungen zu mindest für eine grafische Simulation der Roboterbewegung statt.

Der Erfindung liegt die Aufgabe zugrunde, ein Verfahren zur Kommandosteuerung für eine interaktive Bahnführung eines ki nematisch redundanten Manipulators zu schaffen, mit dem Bahnbeschränkungen durch physikalische Gelenkanschläge sowie maximal erlaubte Gelenkgeschwindigkeiten sicher eingehalten und keine übergeordneten Heuristiken benötigt werden, um die ermittelte Lösung auf Zulässigkeit hinsichtlich der Bahnbe schränkungen zu überprüfen und gegebenenfalls zu korrigie ren.

Gemäß der Erfindung ist diese Aufgabe bei einem Verfahren zur Kommandosteuerung für eine interaktive Bahnführung eines kinematisch redundaten Manipulators gemäß dem Oberbegriff des Anspruchs 1 durch die Merkmale in dessen kennzeichnenden Teil gelöst. Vorteilhafte Weiterbildungen des erfindungsge mäßen Verfahrens sind Gegenstand der auf den Anspruch 1 un mittelbar oder mittelbar rückbezogenen Ansprüche 2 bis 6.

Gemäß der Erfindung, die sich auf ein Verfahren zur Komman dosteuerung für eine interaktive Bahnführung eines kinema tisch redundanten Manipulators der eingangs genannten Art bezieht, ist diese Aufgabe dadurch gelöst, daß

- ausgehend von einer kommandierten Endeffektor-Zielver schiebung und dem aktuellen Istwert (q_i) der Gelenkposition des Manipulators wird unter Berücksichtigung einer Gütefunktion (f(q)), die durch nichtnegative Gewichtungswerte (α_j, β_j) parametriert ist und unter Berücksichtigung von Bahnbeschränkungen durch phy sikalische Gelenkanschläge (q_min, q_max), maximaler Gelenkge schwindigkeit (_max) und der kinematischen Gleichung, welche durch die Jacobi-Matrix repräsentiert ist, eine neue Gelenkposition (q_i+1) berechnet, die die neuen Wer te für die Gelenkregler vorgibt, wobei die Gütefunktion die Summe aus Energiekriterium und Referenzlagekriterium ist, dabei berechnen sich das Energiekriterium aus
(q-q_i)^tdiag(α_j) (q-q_i)
und 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 berechne ten Gelenkpositionswerte (q_i) nahe um diesen Referenzpositi onswert verläuft;
- ausgehend von einem Gelenkpositionswert q₀ als Startpunkt wird auf Basis der Gütefunktion ein zulässiger Optimierungs vektor bestimmt bezüglich aller aktiven Nebenbedingungen die angeben, welche Bahnbeschränkungen erreicht sind, und dieser wird skaliert entsprechend der inaktiven Nebenbedingungen, die angeben, welche Bahnbeschränkungen nicht erreicht sind;
- der skalierte Optimierungsvektor wird auf den im vorigen Iterationsschritt berechneten Gelenkpositionswert addiert;
- und auf der Basis der Gütefunktion und der in der neu be rechneten Gelenkposition aktivierten Nebenbedingungen wird die Optimalität dieser Gelenkpositionswerte bewertet.

Das Verfahren nach der Erfindung ergibt eine Glattheit der berechneten Gelenkbahn und einen geringen Verschleiß des Ro boterantriebes durch Minimierung des lokalen Gelenkposi tionsversatzes (lokale Energiekriterien). Man erzielt "schlanke" Gelenkbahnen um eine Referenzlage (z. B. Nulla ge). Gelenkanschläge werden in vorteilhafter Weise wei testgehend vermieden oder sanft bzw. verschleißarm ange fahren. Dieses Verhalten wird durch einen Verzögerungs effekt der Roboterachsen hervorgerufen, der proportional zum Abstand von der Referenzposition wirkt.

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 Linearbe wegung auf den Endeffektor des Manipulators.

Eine zu einem Zeitpunkt T_i kommandierte kartesische Endef fektor-Zielverschiebung Δx_c kann vom Programmierer in Form eines 6-dimensionalen Inkrementvektors kommandiert werden. Alternativ hierzu kann der Inkrementvektor durch Differenz bildung absoluter Endeffektorkoordinaten bestimmt sein.

Nachfolgend wird die Erfindung anhand der Zeichnungen erläu tert. Es zeigen:

Fig. 1 eine Darstellung des Ein-/Ausgabeflusses der rück wärtskinematik im Echtzeitraster, und

Fig. 2 einen beim Verfahren nach der Erfindung benutzten Al gorithmus für die inverse Kinematik.

In Fig. 1 ist eine Darstellung des Ein-/Ausgabeflusses der Rückwärtskinematik im Echtzeitraster wiedergegeben.

Bei dem Verfahren ist unter Benutzung der Jacobi-Matrix der Kinematik zu einem Zeitpunkt T_i, eine kommandierte Endeffek tor-Zielverschiebung mit Δx_c:=(Δx_t,Δx_r) bezeichnet, welche von einem Programmierer mit Hilfe einer Space-Mouse oder Steuer kugel in Form eines 6-dimensionalen Inkrementvektors komman diert wird. Mit ist jeweils der trans latorische bzw. rotatorische Anteil der kommandierten Endef fektor-Zielverschiebung bezeichnet, während Δx_t ^max bzw. Δx_r ^max jeweils den maximalen (skalarwertigen), von einem Program mierer vorgegebenen translatorischen bzw. rotatorischen kartesischen Lageversatz des Endeffektors pro Abtastzeit ΔT definieren.

Die gewünschte kartesische translatorische bzw. rotatorische Endeffektor-Zielverschiebung Δx_t ^d bzw. Δx_r ^d 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_t ^d,Δx_r ^d) erklärt.

Die Energiekriterien und Referenzlagenkriterien können ent sprechend den vorherrschenden problemspezifischen Anforde rungen geeignet gewichtet werden, indem jeder Achse j zwei positive, vom Programmierer vorgegebene Zahlenwerte, soge nannte Gewichtungswerte α_j,β_j zugewiesen werden. Von diesen Gewichtungswerten dienen der Wert α_j der Gewichtung eines Energiekriteriums, das die Differenz zweier benachbarter be rechneter Gelenkpositionen von Achse j bewertet, und der Wert β_j der Gewichtung eines Kriteriums, das die Auslenkung der Gelenkposition q_j von einem ebenfalls vom Programmierer vorgegebenen Referenzwert q_ref,j bewertet. Der Referenzwert q_ref,j wird genauso vom Programmierer vorgegeben wie die Bahnbe schränkungen betreffenden physikalischen Gelenkanschläge q_min,q_max des Manipulators sowie die Gelenkgeschwindigkeitsbe schränkungen _max.

Da das erfindungsgemäße Verfahren der Optimierung gewichtiger Kriterien im Echtzeittakt unter einer garantierten Einhaltung aller Bahnbeschränkungen dient, wird zu einer gewünschten Endeffektor-Zielverschiebung Δxⁱ _d zum Zeitpunkt T_i-1 während des Zeitintervalls ΔT mit dem sich in einer Initialisierungsphase und einer nachfolgenden Optimierungsphase abgewickelten 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ⁱ _soll+ Δqⁱ erklärt wobei mit qⁱ _soll im Zeitraum ΔT die Gelenkpositionen durch die Gelenkregler entsprechend verfahren werden.

Unter einem zulässigen Gelenkpositionsversatz wird dabei verstanden, daß der mit Δq aktualisierte Gelenkpositionswert den physikalischen Gelenkanschlägen q_min, q_max entsprechend q_min≦q_soll+Δq≦q_max und Δq den Gelenkgeschwindigkeitsbeschränkungen _max entsprechend |Δq|≦_maxΔT genügt. Die Art der Optimalität von Δq ist von einem Programmierer durch die Gütekriteriengewichtung α, β festlegbar. Die Abtastzeit ΔT ist dabei entsprechend ΔT≧max (Δt_q, Δt_r) so bemessen, daß die Rechenzeit Δt_q zur Berechnung eines optimalen zulässigen Ge lenkpositionsversatzes Δ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-Matrix der Kinematik im Punkt q₀ der aktuellen Manipulatorposition, ndof die Anzahl der Gelenke und ε_i:=ⁱ _maxΔT den maximal erlaubten Gelenkpositionsversatz pro Abtastzeit ΔT. Mit

lautet die inkrementelle kinematische Gleichung:

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

Dabei bezeichnet q₀ den jüngst berechneten, zulässigen Soll positionssatz, auf dessen Werte die Achsen des Robotors bereits eingeregelt sind, welche den Istwert darstellen. Definiere weiter:

Damit alle Bahnbeschränkungen erfüllt werden können, ist, gemäß der kinematischen Gleichung (1), die gewünschte Endeffektor-Zielverschiebung Δx_c einer zentrischen Streckung unterworfen, die in Form zweier Skalare beschrieben ist:

0≦p≦1, 0≦q≦1.

Die Größen p, q werden durch das erfindungsgemäße Verfahren so maximiert, daß alle Bahnbeschränkungen eingehalten werden.

Definiere

und die Hilfsgrößen

sowie die Matrix mit n:=ndof+2:

Dabei bezeichnet die Einheitsmatrix. Ferner bezeichnen a_i, i=1, . .,2n+6 die Zeilen von A.

Die Gewichte α, β der Kriterien sind durch die Diagonalmatrix repräsentiert. Deren Hauptdiagonalelemente lauten:

und

mit

und

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, q_k)=0; k max<7; initialisiere mit m_k:=8 die Matrix der aktiven Nebenbedingungen

initialisiere die Indexmenge J_k:=(j_k ¹, j_k ², . . .,j_k ²ⁿ⁺⁶) zur Kennzeichnung 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(Â, m_k, J_k);

initialisiere die orthogonale, reguläre Dreiecksbeziehung von Z:=ΛZ_k:

initialisiere den Vektor

Für g_k wird folgende Kurzschreibweise verwendet:

2(β_i ^scal(yⁱ _k-yⁱ _ref)+α_i ^scalyⁱ _k, p_k-1, q_k-1).

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

I. Berechne die Optimierungsrichtung d_k:
R_k ^Zd_aux=-Q_k ^ZZ_kg_k.
Bestimme hieraus d_aux durch Rückwärtssubstitution.
R_k ^Zd_Z=Q_k ^Zd_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_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 Indexgrößen:
- (a) Falls s_k<1 (Nebenbedingung j_k ⁱ⁰ ist aktiv geworden):
  - i. Falls m_k<n (es noch inaktive Nebenbedingungen):
    - - x_k+1=x_k+s_kd_k, x_k+1=(y_k+1, p_k+1, q_k+1);
      g_k+1=2(β_i ^scal(yⁱ _k+1-yⁱ _ref)+α_i ^scalyⁱ _k+1, p_k+1-1, q_k+1-1).
    - - Aktualisiere Matrixfaktorisierungen und Indexmenge:
      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öhte Iterationsindex)
      Falls k<kmax: Gehe zu Schritt IV.
      Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z:=ΛZ_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, q_k+1);
      g_k+1=2(β_i ^scal(yⁱ _k+1-yⁱ _ref)+α_i ^scalyⁱ _k+1, p_k+1-1, q_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:
    - - Aktiviere Nebenbedingung j_k ⁱ⁰.
      Aktualisiere Matrixfaktorisierungen und Indexmenge:
    - - Prüfe x_k+1auf 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öhte Iterationsindex)
      Falls k<kmax: Gehe zu Schritt IV.
      Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z:=ΛZ_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, q_k+1);
    g_k+1=2(β_i ^scal(yⁱ _k+1-yⁱ _ref)+α_i ^scalyⁱ _k+1, p_k+1, -1, q_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: Gebe eine Richtung frei mit höchstem Optimierungspotential:
  - - Aktualisiere Matrixfaktorisierungen und Indexmenge:
  - - k=k+1 (erhöhte Iterationsindex)
    Falls k<kmax: Gehe zu Schritt IV.
    Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z:=ΛZ_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).
λ_l:=0, i=r+1,. . .,m;
λ_nL:=(λ_l+1,. . .,λ_m);
L^tλ_L=Y^tg.
Bestimme hieraus durch Rückwärtssubstitution.
Definiere den Ausgabewert der Funktion:
λ:=(λ_L, λ_nL).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 Φ₊:
Aktiviere Nebenbedingungen 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 . Bestimme die Householderreflexion so, daß gilt:
mit . Definiere den unitären Transformator gemäß:
Definiere den unitären Transformator:
Aktualisiere Indizes:
Vertausche Nebenbedingungen:
Falls r ≠ r dann
sonst
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 Φ_-:
Inaktiviere Nebenbedingung j^j0.
- - Falls j0<r: (Eliminiere singuläre aktive Nebenbedingung)
  Aktualisiere Indizies:
  Vertausche Nebenbedingungen:
  Aktualisiere Matrixfaktorisierungen:
  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 unteren Eliminationsmatritzen annuliert. Definiere lineare Transformatoren:
- - Falls die letzte Zeile von
- null ist oder falls r=m:
  Aktualisiere Indizies:
- 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:
- - 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:
  dabei bezeichnet _l0 die l0-te Spalte von .
  Aktualisiere Indizies:
  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.
  dabei bezeichnen die _l die Spalten von .
  Ende der Funktion Φ_-.
  Definition der Funktion Φ^A ₀:
1. Definiere:
A⁰:=Â, i:=0.
2. Definiere folgende Matrizenrekursion:
Aⁱ⁺¹:=H_iAⁱ, i≧0.
Der unitäre Transformator ist wie folgt erklärt:
Die Householderreflexion ist so definiert, daß gilt:
mit . Dabei bezeichnen die Vektoren k=1,. . ., n-i die Spalten der Matrix Ferner ist 1≦k0≦n-i der kleinste Index mit ⁱ _k0≠0. Falls kein solches k0 existiert, dann gehe zu Schritt III, sonst gehen zu Schritt IV.
3. Definiere Indizies:
Definiere 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.
Es ist T die Matrix, die durch Streichen der Zeilen r+1 bis n aus Aⁱ⁺¹ entsteht.
Definiere gemäß:
Stop: Ausführung der Funktion Φ₀ ^A ist beendet!
4. Vertausche die Spalte i+k0 mit der Spalte i+1 in Aⁱ.
Setze erhöhe den Zählindex i=i+1 und gehe zu Schritt II.
Ende der Funktion Φ₀ ^A.
Definition der Funktion Φ₀ ^Z:
Bestimme die QR-Dreieckszerlegung von Z:
Dabei bezeichnen eine unitäre Matrix und eine obere Dreiecksmatrix.
Ende der Funktion Φ₀ ^Z.

Claims

1. Verfahren zur Kommandosteuerung für eine interaktive Bahnführung eines kinematisch redundanten Manipulators auf der Basis von mit Hilfe einer Handsteuerkugel oder derglei chen von einem Programmierer kommandierter Endeffektor- Zielverschiebungen in Verbindung mit einer Berechnung von Gelenkpositionswerten entsprechend einem Algorithmus inver ser Kinematik gekennzeichnet durch folgende Merkmale

- ausgehend von einer kommandierten Endeffektor- Zielverschiebung und dem aktuellen Istwert (q_i) der Gelenk position des Manipulators wird unter Berücksichtigung einer Gütefunktion (f(q)), die durch nichtnegative Gewichtungswerte (α_j, β_j) parametriert ist und unter Berücksichtigung von Bahnbeschränkungen durch phy sikalische Gelenkanschläge (q_min, q_max), maximaler Gelenkge schwindigkeit () und der kinematischen Gleichung, welche durch die Jacobi-Matrix repräsentiert ist, eine neue Gelenkposition (q_i+1) berechnet, die die neuen Wer te für die Gelenkregler vorgibt, wobei die Gütefunktion die Summe aus Energiekriterium und Referenzlagekriterium ist, dabei berechnen sich das Energiekriterium aus
(q-q_i)^tdiag(α_j) (q-q_i)
und 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 berechne ten Gelenkpositionswerte (q_i) nahe um diesen Referenzpositi onswert verläuft;
- ausgehend von einem Gelenkpositionswert q_o als Startpunkt wird auf Basis der Gütefunktion ein zulässiger Optimierungs vektor bestimmt bezüglich aller aktiven Nebenbedingungen die angeben, welche Bahnbeschränkungen erreicht sind, und dieser wird skaliert entsprechend der inaktiven Nebenbedingungen, die angeben, welche Bahnbeschränkungen nicht erreicht sind;
- der skalierte Optimierungsvektor wird auf den im vorigen Iterationsschritt berechneten Gelenkpositionswert addiert;
- und auf der Basis der Gütefunktion und der in der neu be rechneten Gelenkposition aktivierten Nebenbedingungen wird die Optimalität dieser Gelenkpositionswerte bewertet.

2. Verfahren nach Anspruch 1, bei welchem unter Benutzung der Jacobi-Matrix der Kinematik, ausgehend von der komman dierten Endeffektor-Zielverschiebung Δx_c:=(Δx_t,Δx_r) zu einem Zeitpunkt T_i, wobei jeweils den transla torischen bzw. rotatorischen Anteil der kommandierten Endef fektor-Zielverschiebung bezeichnen, Δx_t ^max bzw. Δx_r ^max jeweils den maximalen (skalarwertigen), von einem Programmierer vor gegebenen translatorischen bzw. rotatorischen kartesischen Lageversatz des Endeffektors pro Abtastzeit ΔT definieren, eine gewünschte kartesische translatorische bzw. rotatorische Endeffektor-Zielverschiebung Δx_t ^d bzw. Δx_r ^d pro Abtastzeit ΔT wie folgt definiert ist:
(der Betrag |x| eines Vektors x ist dabei und im weiteren durch seine euklidische Norm
gegeben), eine ge wünschte Endeffektor-Zielverschiebung durch Δx_d:=(Δx_t ^d,Δx_r ^d) erklärt ist und die Energiekriterien und Referenzlagenkrite rien entsprechend den vorherrschenden problemspezifischen Anforderungen geeignet gewichtet werden können, indem jeder Achse j zwei positive, vom Programmierer vorgegebenen Zahlenwerte α_j, β_j zugewiesen werden, von denen der Wert α_j der Gewichtung eines Energiekriteriums, das die Differenz zweier benachbarter berechneter Gelenkpositionen von Achse j bewertet, und der Wert β_j der Gewichtung eines Kriteriums, das die Auslenkung der Gelenkposition q_j von dem ebenfalls vom Programmierer vorgegebenen Referenzwert q_ref,j bewertet, dient, der genauso vom Programmierer vorgegeben wird wie die Bahnbeschränkungen betreffenden physikalischen Gelenkanschläge q_min, q_max des Manipulators sowie die Gelenkgeschwindig keitsbeschränkungen _max, daß bei der Erlangung der Verfahrensausgangsgrößen zu einer gewünschten Endeffektor-Zielverschiebung Δxⁱ _d, zum Zeitpunkt T_i-1 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 Δqⁱ berechnet wird, wobei sich der aktuelle Sollwert an die Gelenkregler aus q i+1|soll:=qⁱ _soll+Δqⁱ erklärt (mit qⁱ _soll werden im Zeitraum ΔT die Gelenkpositonen durch die Gelenkregler entsprechend verfahren) und unter dem zulässigen Gelenkpositionsversatz verstanden wird, daß der mit Δq aktualisierte Gelenkpositionswert den physikalischen Gelenkanschlägen q_min, q_max entsprechend q_min≦q_soll+Δq≦q_max und Δq den Gelenkgeschwindigkeitsbeschränkungen _max entsprechend |Δq|≦_maxΔT genügt und die Art der Optimalität von Δq von einem Programmierer 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 Achspositionen an die Sollwerte q_soll benötigen, innerhalb der Zeitspanne ΔT liegen.

3. Verfahren nach Anspruch 2, dadurch gekennzeichnet, daß die inkrementelle kinematische Gleichung unter der Voraussetzung, daß J_i, i=1,. . .,ndof die Spalten der Jacobi-Matrix der Kinematik, ndof die Anzahl der Gelenke und ε_i:=_max ΔT den maximal erlaubten Gelenkpositionsversatz pro Abtastzeit ΔT beschreiben, mit
lautet daß sich aufgrund der Bahnbeschränkungen die Boxbeschränkungen
ergeben, wobei q₀ den jüngst berechneten, zuverlässigen Sollpositionssatz, auf dessen Werte die Achsen des Roboters bereits eingeregelt sind, bezeichnet, daß weiterhin
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 zweier Skalare 0≦p≦1, 0≦q≦1 beschreiben läßt, wobei die Größen p, q so maximiert werden, daß alle Bahnbeschränkungen eingehalten werden, daß
und die Hilfsgrößen
sowie die Matrix , mit n:=ndof+2:
definiert werden, wobei die Einheitsmatrix und a_i, =1,. . .,2n+6 die Zeilen von A bezeichnen, daß die Gewichte α, β der Kriterien durch die Diagonalmatrix repräsentiert werden, deren Hauptdiagonalelemente
für 1≦i≦n-2 und für n-2<i≦n, mit
lauten.

4. Verfahren nach Anspruch 3, dadurch gekennzeichnet, daß beim verwendeten Algorithmus der inversen Kinematik eine Initialisierungsphase durchgeführt wird, die folgendermaßen ausgestaltet ist:
Zählindex: k=0; Startwert: x_k=(y_k, p_k, q_k)=0; k max<7; initialisiere mit m_k:=8 die Matrix der aktiven Nebenbedingungen
initialisiere die Indexmenge J_k:=(j_k ¹, j_k ², . . ., j 2n+6|k) zur Kennzeichnung 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 (Â, m_k, J_k);
initialisiere die orthogonale, reguläre Dreieckszerlegung von Z:=ΛZ_k:
initialisiere den Vektor
g0∈Rⁿ: g_k:= 2(β_i ^scal(yⁱ _k-yⁱ _ref)+α_i ^scalyⁱ _k, p_k-1, q_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_k ^Zd^daux=-Q_k ^ZZ_kg_k.
Bestimme hieraus d_aux durch Rückwärtssubstitution.
R_k ^Zd_z=Q_k ^Zd_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_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 Indexgrößen.
- (a) Falls s_k<1 (Nebenbedingung j_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, q_k+1);
      g_k+1=2(β_i ^scal(yⁱ _k+1-yⁱ _ref)+α_i ^scaly_k+1, p_k+1, -1, q_k+1-1).
    - - Aktualisiere Matrixfaktorisierungen und Indexmenge: 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öhte Iterationsindex)
      Falls k<kmax: Gehe zu Schritt IV.
      Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z:=ΛZ_k+1
      Gehe zu Schritt I.
    ii. Falls m_k=n (Ecke des zuläsigen Bereichs erreicht. Austausch aktiver Nebenbedingungen erforderlich):
    - - x_k+1=x_k+s_kd_k, x_k+1=(y_k+1, p_k+1, q_k+1);
      g_k+1=2(β_i ^scal(y_k+1-y_ref)+α_i ^scaly_k+1, p_k+1, -1, q_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:
    - - Aktiviere Nebenbedingungen j_k ⁱ⁰.
      Aktualisiere Matrixfaktorisierungen und Indexmenge:
    - - Prüfe x_k+1 auf Optimalität:
      Falls Z^t _k+1 g_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<kmax: Gehe zu Schritt IV.
      Sonst: Bestimme die orthonogale, reguläre Dreieckszerlegung von Z:=ΛZ_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, q_k+1);
      g_k+1=2(β_i ^scal(y_k+1-y_ref)+α_i ^scaly_k+1, p_k+1, -1, q_k+1-1).
      -Aktualisiere Matrixfaktorisierungen und Indexmene:
      (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: Gebe eine Richtung frei mit höchstem Optimierungspotential:
    - - Aktualisiere Matrixfaktorisierungen und Indexmenge:
    - - k=k+1 (erhöhe Iterationsindex)
      Falls k<kmax: Gehe zu Schritt IV.
      Sonst: Bestimme die orthogonale, reguläre Dreieckszerlegung von Z:=ΛZ_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).
λ_l:=0, i=r+1,. . .,m;
λ_hL:=(λ_r+1,. . .,λ_m);
L^tλ_L=Y^tg.
Bestimme hieraus durch Rückwärtssubstition. 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 Φ₊:
Aktiviere Nebenbedingungen jⁱ⁰. Es bezeichnen 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 . Bestimme die House holderreflexion so, daß gilt:
mit . Definiere den unitären Transformator gemäß:
Definiere den unitären Transformator:
Aktualisiere Indizies:
Vertausche Nebenbedingungen:
Falls r ≠ r dann
sonst
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 Φ_-:
Inaktiviere Nebenbedingung j^j0..
- - - - Falls j0<r: (Eliminiere singuläre aktive Nebenbedingung)
      Aktualisiere Indizies:
      r:=r;
      m:=m-1;
      Vertausche Nebenbedingungen: Aktualisiere Matrixfaktorisierungen:
      dabei bezeichnen S_i die Spalten von S.
    - - Falls j0≦r: (Eliminiere reguläre aktive Nebenbedingungen) 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 annulliert. Definiere lineare Transformatoren:
    - - Falls die letzte Zeile von
    - null ist oder falls r=m:
      Aktualisiere Indizies:
      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:
      - 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:
      dabei bezeichnet _l0 die l0-te Spalte von .
      Aktualisiere Indizes:
      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.
      dabei bezeichnen die _l die Spalten .
      Ende der Funktion Φ_-.
      Definition der Funktion Φ₀ ^A:
1. Definiere:
A⁰:=, i:=0.
2. Definiere folgende Matrizenrekursion:
Aⁱ⁺¹:=H_iAⁱ, i≧0.
Der unitäre Transformator ist wie folgt erklärt:
Die Householderreflexion ist so definiert, daß gilt:
mit . Dabei bezeichnen die Vektoren die Spalten der Matrix :
Ferner ist 1≦k0≦n-i der kleinste Index mit ãⁱ _k0≠0. Falls kein solches k0 existiert, dann gehe zu Schritt III, sonst gehe zu Schritt IV.
3. Definiere Indizies:
Definiere 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.
Es ist T die Matrix, die durch Streichen der Zeilen r+1 bis n aus Aⁱ⁺¹ entsteht.
Definiere gemäß:
Stop: Ausführung der Punkte Φ₀ ^A ist beendet!
4. Vertausche die Spalte i+k0 mit der Spalte i+1 in Aⁱ.
Setze erhöhe den Zählindex i=i+1 und gehe zu Schritt II.
Ende der Funktion Φ₀ ^A.
Definition der Funktion Φ₀ ^Z:
Bestimme die QR-Dreieckszerlegung von Z:
Dabei bezeichnen eine unitäre Matrix und eine obere Dreiecksmatrix.
Ende der Funktion Φ₀ ^Z.

5. Verfahren nach Anspruch 2, dadurch gekennzeichnet, daß die zu einem Zeitpunkt T_i kommandierte kartesische Endeffektor-Zielverschiebung Δx_c vom Programmierer in Form eines 6-dimensionalen Inkrementvektors kommandiert wird.

6. Verfahren nach Anspruch 2, dadurch gekennzeichnet, daß der Inkrementvektor der zu einem Zeitpunkt T_i kommandierten kartesischen Endeffektor-Zielverschiebung Δx_c vom Programmierer durch Differenzbildung absoluter Endeffektorkoordinaten bestimmt wird.