-
Die Erfindung betrifft eine Bewegungs-Steuervorrichtung für
einen Servomechanismus und insbesondere ein digitales
Wegerzeugungs- und Spurhalteverfahren zum raschen Bewegen eines
positionierbaren Glieds zu einer befohlenen Position mit
vernachlässigbarem Folgefehler und im wesentlichen keinem
Überschießen, unabhängig von Geschwindigkeit, innerhalb der
Fähigkeit des Servomotors.
-
In breiter Hinsicht betrifft die Erfindung einen
Servomechanismus des Typs, der ein Ausgabeglied wie einen Roboterarm
in Übereinstimmung mit einer Zielposition bewegt.
Funktionell enthält dies das Erfassen der gegenwärtigen Position
des Ausgabegliedes, das Definieren einer erforderlichen
Trajektorie oder eines Weges zum Erreichen der Zielposition,
das Bestimmen eines Betätigerbefehls aufgrund des
Positionsfehlers und der erforderlichen Trajektorie, und das
Aktivieren des Servomechanismus entsprechend dem Betätigerbefehl.
Wenn der Servomechanismus ein Gleichstrom-Servomotor ist,
wird der Betätigerbefehl typischerweise in Hinsicht auf den
Ankerstrom oder die -Geschwindigkeit bestimmt und wird mit
Benutzung von Regelverfahren (mit einer geschlossenen
Regelschleife) ausgeführt.
-
Ein Bewegungs-Steuerverfahren nach der vorliegenden
Erfindung wird gekennzeichnet durch die im kennzeichnenden
Abschnitt des Anspruchs 1 angegebenen Merkmale.
-
Insbesondere bezieht sich die vorliegende Erfindung auf ein
Verfahren und eine Vorrichtung zum Aktivieren eines
Servomechanismus zur Verfolgung einer Trajektorie dritter Ordnung
mit im wesentlichen keinem Spurfehler. Die Spurhalte- oder
Folgefehler sind ein direktes Ergebnis der üblichen
Rückkoppel-Regelvorrichtung, bei der die Betätiger-Befehle als eine
Funktion des Fehlers zwischen der befohlenen Position
(Trajektorie) und der tatsächlichen Position bestimmt werden.
Dies enthält sog. Proportional/Differential-Regelungen, bei
denen der Betätigerbefehl als eine lineare Kombination des
Positionsfehlers und der gemessenen Motor-Geschwindigkeit
bestimmt wird.
-
Folgefehler neigen dazu, mit zunehmender Belastung und auch
mit zunehmender befohlener Geschwindigkeit und
Beschleunigung zuzunehmen. Der Fehler kann bedeutsam werden, da die
Befehlspositionen koordiniert sind (z.B. geradlinig),
während der Folgefehler es nicht ist. Folgefehler stellen eine
Abweichung des Ausgabegliedes von der beabsichtigten
Trajektorie dar, und die Abweichung wird üblicherweise innerhalb
"annehmbarer" Grenzen gehalten durch Auferlegen zusätzlicher
Begrenzungen auf die Spitzen-Geschwindigkeit und
-Beschleunigung der Trajektorie. Das ergibt eine weitere
Unterbenutzung des Servomechanismus und erhöht weiter die zum
Erreichen der Zielposition erforderliche Zeit.
-
Diese Erfindung überwindet das Problem der Spurhalte- oder
Folgefehler mit einem neuartigen adaptiven
Vorwärtseinspeise-Regelverfahren, das befähigt ist, wohl definierte Wege
dritter Ordnung im wesentlichen mit Fehler Null zu
verfolgen. Ein Motorspannungsbefehl Vm wird entsprechend der Summe
eines Rückkoppel-Terms aufgrund des Positionsfehlers und
eines Vorwärts-Einspeiseterms aufgrund der Dynamik der
befohlenen Position bestimmt. Der Rückkoppelterm stellt die
Gesamtsystem-Stabilität sicher, während der
Vorwärts-Einspeiseterm im wesentlichen Folgefehler Null sicherstellt
innerhalb der dynamischen Fähigkeit des Servomechanismus
durch Vorwegnahme der Dynamik der befohlenen Position.
-
Bei einem Servomotor unter einfacher Trägheitsbelastung ist
der Vorwärts-Einspeiseterm in Form einer linearen
Kombination aus den ersten drei Befehlsabgeleiteten vorhanden
-- d.h. der befohlenen Geschwindigkeit, Beschleunigung und
Änderungsrate der Beschleunigung. Die Koeffizienten derartiger
Abgeleiteter sind eindeutig bestimmt durch die
physikalischen Eigenschaften des Servomotors und der Last, wie auch
durch die Abtastrate bei Steuerung und
(Rückkoppel-)Regelung. Veränderungen der Merkmale von Motor zu Motor und von
Tag zu Tag und damit die Koeffizienten werden ausgeglichen
durch adaptives Nachstellen der Koeffizienten in Real zeit
aufgrund des Spurfehlers und der Befehlsabgeleiteten. Als
ein Ergebnis kann die Bewegungs-Steuervorrichtung die
Trajektorie dritter Ordnung virtuell mit Fehler Null verfolgen.
-
Die Vorwärts-Einspeisesteuerung dieser Erfindung wird hier
in Verbindung mit einem Verfahren zur Erzeugung eines Weges
dritter Ordnung beschrieben, welche Gegenstand der
gleichzeitig eingereichten Patentanmeldung EP 88 303 067.8 (unser
Bezugszeichen: MJD/3036) ist, inzwischen EP-Patent Nummer
0 289 150.
In der Zeichnung sind
-
Fig. 1 eine schematische Darstellung einer Bewegungs-
Steuervorrichtung nach dieser Erfindung.
-
Fig. 2-6 graphische Darstellungen, auf die bei der
Beschreibung der theoretischen Wegerzeugungs-
Lösung Bezug genommen wird.
-
Fig. 7-23 für Computerprogramm-Instruktionen
repräsentative Flußdiagramme, die durch die Steuereinheit
der Fig. 1 beim Ausführen der erfindungsgemäßen
Wegerzeugungs-Funktion durchgeführt werden.
-
Fig. 24-28 graphische Darstellungen, die den Betrieb der
-
Fig. 24-28 graphische Darstellungen, die den Betrieb der
Erzeugungsfunktion von Wegen dritter Ordnung
bildlich darstellen.
-
Fig. 29 ein Steuervorrichtungs-Schaubild der adaptiven
Vorwartseinspeise-Regelung nach dieser Erfindung.
-
Fig. 30 ein für Computerprogramm-Instruktionen, welche
durch die Steuereinheit der Fig. 1 bei der
Mechanisierung der adaptiven
Vorwärtseinspeise-Regelung nach dieser Erfindung ausgeführt werden,
repräsentativen Flußdiagramm
-
Fig. 31 und 32 graphische Darstellungen der bei der
Benutzung der vorliegenden Erfindung
beobachteten Verbesserungen.
-
In der Zeichnung und insbesondere in Fig. 1 bezeichnet
Bezugszeichen 10 schematisch ein positionierbares Glied eines
Servomechanismus nach dieser Erfindung. Das positionierbare
Glied 10 kann die Form eines Armes eines Industrie-Roboters
wie bei der dargestellten Ausführung, oder irgendeines
positionsgesteuerten (Ausgangs-) Glieds wie eines Plotterarmes,
einer Motor-Drosselklappe usw. annehmen. Ein konventioneller
Gleichstrom-Servomotor 12 bestimmt einen Servomechanismus,
der über eine Welle 14 starr mit dem positionierbaren Glied
10 gekoppelt und zur Beaufschlagung durch einen
(Gleichspannungsbetriebs-)Leistungsverstärker 16 ausgelegt ist, um eine
Bewegung des positionierbaren Gliedes 10 zu bewirken. Der
Gleichstrom-Servomotor 12 ist auch mit einem
Positions-Rückmeldefühler 18 wie einem Weggeber oder optischen Kodierer
gekoppelt, der ein für die Motorposition bezeichnendes
Elektrosignal an Leitung 20 schafft.
-
Das Bezugszeichen 22 bezeichnet allgemein eine
erfindungsgemäße computergestützte Steuereinheit zum Steuern der
Ausgangsspannung des Leistungsverstärkers 16 in Abhängigkeit
von dem Ausgangssignal des Positions-Rückmeldefühlers 18, um
so das positionierbare Glied 10 in Minimal zeit von einer
Anfangsposition zu einer Zielposition in den Ruhezustand zu
bewegen, im wesentlichen ohne Überschießen oder Folgefehler.
Die Steuereinheit 22 stellt ein Aggregat üblicher Elemente
dar einschließlich einer zentralen Bearbeitungseinheit,
eines kristallgesteuerten Taktgebers, eines
Festwertspeichers und eines Speichers mit wahlfreiem Zugriff, digitaler
und analoger Eingabe/Ausgabe-Anschlüsse und D/A- und A/D-
Wandlerkanäle. Die Steuereinheit 22 unterstützt auch eine
Benutzer-Schnittstelle 24 wie ein Videoanzeige-Terminal, um
die Eingabe von Befehlspositionen und Vorrichtungsparametern
durch die Bedienungsperson zu ermöglichen.
-
Die Steuereinheit 22 führt bei der Ausführung der Ziele
dieser Erfindung zwei primäre Funktionen aus -- Wegerzeugung
und Wegverfolgung Wegerzeugung bezieht sich auf die
Ableitung einer befohlenen Trajektorie im Phasenraum zwischen
einer Anfangs- und einer Zielposition. Wegverfolgung bezieht
sich auf die Steuerung der Motorspannung, um die Bewegung
des positionierbaren Glieds 10 entsprechend der gewünschten
Trajektorie zu bewirken. Da die Wegerzeugungs-Routine
rekursiv ist, führt die Steuereinheit 22 periodisch beide
Funktionen aus, während sie das positionierbare Glied 10 bewegt.
Bei jeder Ausführungsperiode aktualisiert die Wegerzeugungs-
Routine den gewünschten Weg, und die Wegverfolgungs-Routine
bestimmt die erforderliche Motorspannung für die genaue
Verfolgung des Weges.
-
Es sollte verstanden werden, daß die Wegfolgefunktion dieser
Erfindung vorteilhafterweise mit einer unterschiedlichen und
einer üblichen Wegerzeugungs-Routine abläuft, solange die
Änderungsrate der Beschleunigung (Stoß) im wesentlichen
begrenzt bleibt. Wie vorher angesprochen, wird sie später in
Verbindung mit einer Minimalzeit-Wegerzeugungsfunktion
dargestellt, die der Gegenstand der vorher erwähnten
gleichzeitig eingereichten Patentanmeldung ist. Die Wegerzeugungs-
und die Wegfolge-Funktion werden nachher vor einer
Diskussion der dargestellten Ausführung separat angesprochen, bei
der beide Funktionen durch die einzige computergestützte
Steuereinheit 22 ausgeführt werden.
Erzeugung eines Weges dritter Ordnung
-
Wie jeder gute Weg, muß der Weg y dritter Ordnung für den
Servomechanismus dieser Erfindung im Rahmen der Fähigkeiten
des Gleichstrom-Servomotors liegen. Die Grenzen des
Gleichstrom-Servomotors sind definiert durch Geschwindigkeits-,
Beschleunigungs- und Stoßgrenzwerte V, A bzw. W. Der
Geschwindigkeits-Grenzwert V entsteht aus der Motorspannungs-
Begrenzung und der Beschleunigungs-Grenzwert A aus der
Motorstrom-Begrenzung. Der Stoßgrenzwert W entsteht aus der
Fähigkeit des Servomechanismus, mechanische Nachgiebigkeit
zu tolerieren, und der Fähigkeit der Stromquelle, Strom mit
der erforderlichen Anderungsrate zu liefern. Zusätzlich
dazu, daß Motor- und Versorgungs-Verhaltensbegrenzungen
nicht verletzt werden dürfen, muß der erfindungsgemäße Weg
die Übergangszeit zu einer angegebenen Zielposition
minimalisieren und in der Zielposition zur Ruhelage führen. Wenn T
die gesamte Übergangszeit und t die augenblickliche Zeit
ist, kann das Problem algebraisch wie folgt festgestellt
werden:
-
für 0 ≤ t ≤ T und für vorgegebene Grenzen von V, A und W.
-
Unter Anwendung der konventionellen Steuerungstheorie kann
die Zeit T mit Extremwert-Steuerung von y"' minimiert
seinen Minimalwert -W oder einen Wert Null an. Algebraisch:
-
in Abhängigkeit von y(t), y'(t) und y"(t). Die Theorie gibt
auch an, daß bei einer bestimmten Lösung höchstens zwei
Umschaltungen zwischen +W und -W vorhanden sind. Siehe Lee und
Markus, Foundations of Optimal Control Theory, John Wiley
and Sons, Inc., 1968, worauf hier zu Vergleichszwecken
verwiesen wird.
-
Eine Diskussion der theoretischen Lösung mit dem Tiel "A New
Approach to Minimum Time Robot Control" wurde durch den
Erfinder verfaßt und veröffentlicht in den Proceedings of the
Winter Annual Meeting of the ASME, 17.-22. November 1985,
Robotics and Manufacturing Automation, PED Bd. 15, worauf
gleichfalls hier zu Vergleichszwecken verwiesen wird.
-
Die Extremwertform der Minimalzeitlösung bedeutet, daß der
(y, y', y")-Phasenraum in drei Bereiche unterteilt werden
kann: einen, in welchem y"' (t) = +W, einen, in welchem
y"'(t) = 0 und einen, in welchem y"'(t) = -W. Der Term
y"'(t) ist immer dann Null, wenn der Geschwindigkeits-
Grenzwert V oder der Beschleunigungs-Grenzwert A steuert.
-
Wird zuerst die Bedingung behandelt, bei der y"' = +W ist,
und angenommen, daß die Anfangsposition y = y&sub0;, die
Anfangs-Geschwindigkeit y' = y&sub0;' und die
Anfangs-Beschleunigung y" = y&sub0;" sind, ergibt eine sukzessive Integration von
y"':
-
Eine Kombination der Ausdrücke (1) und (2) und eine
Darstellung der Anfangsbedingungen als eine Konstante K ergibt:
lung der Anfangsbedingungen als eine Konstante K ergibt:
-
y' = (y"²/2W) + K (4)
-
wodurch eine Familie von nach oben konkaven Parabeln in der
(y", y')-Ebene bestimmt wird, wie graphisch durch die
Spuren 30-32 in Fig. 2 dargestellt. Da y"' > 0 wächst y"
und die Parabeln werden, wie bezeichnet, von links nach
rechts durchlaufen.
-
Da y' kleiner als der oder gleich dem
Motorgeschwindigkeits-Grenzwert V sein muß, wird die untere Begrenzung y' ≥
-V in der (y", y')-Ebene gegeben durch den Ausdruck:
-
y' = (y"²/2W) - V (5)
-
Dies ist die Parabel 32 der Fig. 2, welche den Punkt y' = -V
schneidet.
-
Eine gleichartige Analyse für die Bedingung y"' = -W ergibt
den Ausdruck:
-
y' = -(y"²/2W) + K (6)
-
der eine Familie von nach unten konkaven Parabeln in der
(y", y')-Ebene definiert, wie graphisch durch die Kurven
34-38 in Fig. 2 dargestellt. Da y"' ≤ 0, nimmt y" ab und
die Parabeln werden, wie angezeigt, von rechts nach links
durchlaufen.
-
In diesem Falle (y"' = -W), ergibt die
Geschwindigkeits-Begrenzung y' ≤ +V in der oberen Grenze in der (y", y')-
Ebene, wie durch den Ausdruck gegeben:
-
y' = -(y"²/2W) + V (7)
-
Dies ist die Parabel 34 der Fig. 2, welche den Punkt y' = +V
schneidet.
-
Wenn die Parabeln 32 und 34 mit der Begrenzung kombiniert
werden, daß y" kleiner als der oder gleich dem
Motorbeschleunigungs-Grenzwert A sein soll, wird ein Bereich D von
zulässigen Kombinationen von y' und y" definiert. Dieser
Bereich ist graphisch in Fig. 3 abgebildet, wo die obere
Begrenzung durch die Parabel 34, die untere Begrenzung durch
die Parabel 32, die linke Begrenzung durch den
Beschleunigungs-Grenzwert y" = -A und die rechte Begrenzung durch den
Beschleunigungs-Grenzwert y" = +A gegeben ist. Der Bereich
D stellt so eine Wegbegrenzung aufgrund der physikalischen
Begrenzungen des Servosystems dar. Er kann mathematisch wie
folgt ausgedrückt werden:
-
Die auf Beschleunigung basierenden Grenzwerte sind linear
bei y" = ±A, jedoch sind die auf Geschwindigkeit
basierenden Grenzwerte das nicht, da sie ja auf Beschleunigung
bezogen sind. Sobald ein Punkt 1 an der oberen Begrenzung
getroffen wird (wie durch den Weg 40), muß die maximale
Abbremsrate (y"' = -W) aufgerufen wurden, um ein
Überschreiten des Geschwindigkeits-Grenzwertes V zu vermeiden. Die
Beschleunigung ist Null, wenn der Geschwindigkeits-Grenzwert
V tatsächlich beim Punkt 2 erreicht wird. Eine gleichartige
Analyse trifft auf die untere Begrenzung zu.
-
Das Hauptziel der Wegerzeugung besteht darin, den
Minimalzeitweg zu definieren, wobei berücksichtigt werden muß, daß
der physikalische Begrenzungsbereich nach Fig. 3 eine
Anderung der optimalen Trajektorie bewirken kann. Um die
Targetposition Y im Ruhezustand zu treffen, muß der Weg y in der
(y", y')-Ebene eine Parabel durchlaufen, welche den Punkt
y' = y" = 0 trifft. Die Endbedingungen sind gegeben durch
y(T) = Y und y' (T) = Y" (T) = 0. Die allgemeine Lösung für
diese Begrenzung wird erreicht durch Kombinieren der
Ausdrücke (1) und (2) und Einsetzen in den Ausdruck (3) für
y"' = + W, was die Ausdrücke ergibt:
-
Die Ausdrücke (9) und (10) definieren eine "End-Annäherungs-
"C-Kurve in dem (y", y', y)-Phasenraum, welche die Gruppe
aller Punkte (y", y', y) enthält, welche zu dem Ziel von
y = Y bei Ruhe mit y"' = ± W angesteuert werden können.
Eine graphische Darstellung der in die (y", y')-Phasenebene
projizierten Kurve C ist in Fig. 4 gegeben. Die Kurve C ist
effektiv in zwei miteinander unverbundene Bereiche geteilt:
einen durch die Spur 42 bezeichneten Bereich C&spplus; und einen
durch die Spur 44 bezeichneten Bereich C&supmin;. Der Bereich C&supmin;
stellt eine End-Annäherungskurve zum Auftreffen auf die
Zielposition Y im Ruhezustand mit y"' = -W dar; der Bereich
C&spplus; stellt eine End-Annäherungskurve zum Auftreffen auf die
Zielposition Y im Ruhezustand mit y"' = +W dar.
-
Die Fläche S&spplus; wird definiert als die Gruppe aller Punkte
(y", y', y), die zu der Kurve C&supmin; mit y"' = +W gelenkt
werden können, und eine Fläche S&supmin; als die Gruppe aller
Punkte (y", y', y), die zu der Kurve C&spplus; mit y"' = -W
gelenkt werden können. In jedem Fall wird das Vorzeichen des
Ausdrucks y"' umgeschaltet, wenn der jeweilige Bereich der
C-Kurve getroffen wird. Die Fläche S und ihre Beziehung zur
Kurve C ist graphisch in Fig. 5 abgebildet.
-
Mathematisch kann die Fläche S&spplus; definiert werden als die
Gruppe aller Punkte (y", y', y), die so sind, daß:
-
In gleicher Weise kann die Fläche S&supmin; definiert werden
als die Gruppe aller Punkte (y", y', y), so daß:
-
Da die Minimalzeitlösung höchstens zwei Umschaltungen von
zwischen +W und -W erfordert, folgt, daß für Punkte
über der Fläche S, y"' = -W benutzt werden sollte, bis die
S&spplus;-Fläche getroffen wird. In diesem Fall wird y"' = +W
dann an der S&spplus;-Fläche gebraucht, bis die Kurve C&supmin; getroffen
wird, worauf y"' = -W benutzt wird, bis das Ziel Y
getroffen wird. In gleicher Weise sollte für Punkte unter der
Fläche S y"' = +W benutzt werden, bis die Fläche S&supmin;
getroffen wird. In einem solchen Fall wird dann y"' = -W an der
S&supmin;-Fläche benutzt, bis die Kurve C&spplus; getroffen wird, wonach
y"' = +W benutzt wird, bis das Ziel Y getroffen wird.
-
Der Rest der theoretischen Lösung ist auf die Definierung
von y"' gerichtet (d.h. +W, -W oder 0) in Hinsicht auf die
Ausdrücke für die Oberfläche S die Kurve C und den Bereich
D. Intuitiv und von den Ausdrücken (11) - (14) hängt die
Nähe eines Punkts (y", y', y) zu der Oberfläche S für
irgendeine bestimmte Zielposition Y von y", y' und y-Y ab,
d.h. von der Beschleunigung, der Geschwindigkeit und dem
relativen Abstand zum Ziel. Das Ziel ist dann, y"' in der
Form
-
zu definieren.
-
In Hinblick auf das Vorstehende und ohne Bezug auf die
Geschwindigkeits- und Beschleunigungs-Begrenzungen V bzw. A
können wir y"' wie folgt definieren:
-
Die Kurve C und die Fläche S wurden mit der Annahme
definiert, daß, sobald die Trajektorie an der Fläche S ist, der
Beschleunigung-Grenzwert nicht verletzt werden kann. Diese
Bedingung ist graphisch in Fig. 6 dargestellt, welche die
(y", y')-Projektion einer repräsentativen Trajektorie 46
und einer End-Annäherungs-C-Kurve 48/50 darstellt, welche
parabolische obere und untere Grenzwerte 34 bzw. 32 des
D-Bereichs schneidet. Bei Annahme einer anfänglichen
Ruhelage unter der Fläche S und y"' = +W nimmt die Trajektorie
46 an Geschwindigkeit und Beschleunigung zu, bis die Fläche
S an der Stelle 1 getroffen wird. Danach wird y"' = -W
benutzt und die Trajektorie 46 folgt der Fläche S&supmin;, bis sie die
End-Annäherungs-C-Kurve 48 trifft. Wenn die Überschneidung
erfolgt, wird der Ausdruck y"' auf +W umgeschaltet und die
Trajektorie folgt der projizierten C-Kurve 48 bis zur
Zielposition
-
Infolge der Beziehung zwischen der projizierten C-Kurve 48
und dem D-Bereich wird keine Trajektorie innerhalb der
oberen und unteren Grenzwerte 34 bzw. 32 den
Beschleunigungs-Grenzwert y" = A verletzen. Diese Beziehung kann
garantiert werden durch entsprechende Begrenzung des Wertes
von W -- insbesondere muß das Produkt WV kleiner oder gleich
A² sein. Wenn auch bequem, kann die Benutzung einer solchen
Begrenzung allgemein nicht erwünscht sein, da sie die zum
Treffen einer Zielposition erforderliche Zeit erhöht.
-
Mit einem größeren Wert für W (d.h. WV > A²) flacht die
projizierte C-Kurve ab und schneidet die Beschleunigungs-
Grenzwerte des D-Bereichs, wie durch die gestrichelte Spur
52/54 dargestellt. In einem solchen Fall kann der
Beschleunigungs-Grenzwert A auftreten, bevor die Trajektorie die
C-Kurve trifft, wie in Fig. 6 durch die gestrichelte Spur 56
gezeigt. Wenn dies geschieht, muß der Term y"' gleich Null
gesetzt werden, um eine Verletzung des Grenzwerts zu
verhindern, und die Trajektorie folgt dem vertikalen
Beschleunigungs-Grenzwert, bevor sie die Endnannäherungs-C-Kurve 52
trifft und ihr zur Zielposition folgt.
-
Die vorstehend beschriebene Grenzwertsituation wird
erfindungsgemäß behandelt durch (1) Modifizieren der C-Kurve zur
Bildung einer neuen als bezeichneten Kurve und (2)
Modifizieren der S-Fläche zum Bilden einer neuen als S
bezeichneten Fläche. Die abgewandelte oder modifizierte
Projektionskurve enthält ein parabolisches Segment, wie die normale
Kurve C, und ein vertikales Segment, das durch den
Beschleunigungs-Grenzwert A geschaffen ist. Die abgewandelte
Fläche stellt die Gruppe aller Punkte (y", y', y) dar,
welche mit y"' = ± W zu der modifizierten Kurve gelenkt
werden können.
-
Wie vorstehend angezeigt, wird der Beschleunigungs-Grenzwert
nur bei Verfolgen der C&spplus;-Kurve angetroffen, wenn das
Produkt WV > A² ist, wobei V die mit y"' = -W erreichte
Maximal-Geschwindigkeit ist. Da die parabolischen Trajektorien
in der (y", y')-Ebene unter y"' = -W von der Form sind:
-
y' + (y")²/2W = konstant
-
-- siehe Ausdruck (7) -- wobei die Konstante die mit y"' =
-W erzielte Maximal-Geschwindigkeit ist, wird der
Beschleunigungs-Grenzwert angetroffen, wenn
-
Aus den Ausdrücken (9) und (10) wird die Überschneidung der
vorher definierten Kurve C&spplus; mit dem
Beschleunigungs-Grenzwert von y" = A gegeben durch die Ausdrücke:
-
Das vertikale Segment der modifizierten Projektionskurve
C&spplus; entspricht dann der Gruppe von Punkten (y", y', y),
welche zu dem Überschneidungspunkt gelenkt werden können,
der durch die Ausdrücke (19) und (20) definiert ist mit y"
= -A und y"' = 0. Integrieren von y" = -A, um y' und y zu
finden, und Ersetzen der Terme y' und y" von den Ausdrücken
(19) und (20) ergibt einen Ausdruck für das vertikale
Segment wie folgt:
-
Zusammengefaßt kann die modifizierte C&spplus;-Kurve definiert
werden als die Gruppe aller Punkte (y", y', y) bei denen
parabolisches Segment
Vertikal-Segment
-
Eine gleichartige Analyse zeigt, daß die modifizierte
&supmin;-Kurve definiert werden kann als die Gruppe aller Punkte (y",
y', y) bei denen:
parabolisches Segment
Vertikal-Segment
-
Wie vorstehend bezeichnet, ist die modifizierte Fläche
&supmin; die Gruppe aller Punkte (y", y', y), die zu der
modifizierten Kurve &spplus; mit y"' = -W gelenkt werden können.
Falls
-
erfüllt ist, wird der Beschleunigungs-Grenzwert A nicht
getroffen und die unmodifizierte Fläche S ist gültig.
Wenn jedoch
-
erfüllt ist, wird der Beschleunigungs-Grenzwert A
angetroffen
und die Fläche S muß modifiziert werden ( &supmin;), um die
Gruppe aller Punkte darzustellen, die zu dem
Vertikalsegment der Kurve &spplus; mit y"' = -W gelenkt werden können.
-
Ist die Bedingung gegeben, daß y"' = -W, ergibt eine
Integration, um y", y' und y zu finden:
-
wobei tc der Zeitpunkt ist, zu dem die Trajektorie die
modifizierte Kurve &spplus; trifft. Da y"(tc) = -A,
-
Kombinieren der Ausdrücke (22) - (26) und Unterdrücken der
Anfangsbedingungs-Indices ergibt einen Ausdruck für die
modifizierte Fläche &supmin; wie folgt:
-
wobei tc gegeben ist durch:
-
Eine gleichartige Analyse mit Bezug auf die modifizierte
Kurve &supmin; zeigt, daß die modifizierte Fläche &spplus; gegeben ist
durch den Ausdruck:
-
wobei tc gegeben ist durch den Ausdruck:
-
Unglücklicherweise ist die theoretische Lösung nicht
ausgelegt zur direkten Mechanisierung in einer digitalen oder
diskret abgetasteten Anordnung. Die Schwierigkeit liegt in
der Erfassung der exakten Überschneidung der Trajektorie mit
den Flächen, Kurven und Begrenzungen, bei denen y"'
geändert werden muß. Das gilt mit Bezug auf die Oberfläche S,
die Kurve C und die Begrenzungsflächen des Bereichs D. Da
eine digitale Anordnung mit einer diskreten
Zeitaktualisierungsrate arbeitet, wird der exakte Zeitpunkt der
Flächenüberquerung in aller Wahrscheinlichkeit nicht mit einem
Abtastzeitpunkt zusammenfallen. Wenn beispielsweise die
Vorrichtung einen Abtastintervall von dt Sekunden besitzt
und y über der Fläche S liegt, wird das Umschalten von y"'
von +W zu -W wahrscheinlich zu spät (bis zu dt Sekunden)
auftreten und die Trajektorie von (y", y', y) wird die
Fläche S überschießen. Anders gesagt, das Triple (y", y',
y-Y) liegt nach dem Umschalten entweder über oder unter der
Fläche S. Möglicherweise ergibt sich dadurch ein
Begrenzungszyklus in der Wegposition y, bei dem y"' zwischen -W
und +W abwechselt -- eine unannehmbare Betriebsart.
-
Der vorstehend beschriebene Betrieb wird erfindungsgemäß
vermieden durch eine Approximation der Wegausdrücke, welche
die Aufgabe, das Ziel Y im Ruhezustand zu treffen, erreicht,
ohne die Vorrichtungs-Begrenzungen zu verletzen. Die Zahl
der Berechnungen pro Zeitstufe wird minimalisiert, um die
Realzeit-Wegerzeugung zu ermöglichen.
-
Bei einer digitalen Ausführung werden die Terme y, y', y",
y"' alle dt Sekunden aktualisiert. Es wird angenommen, daß
y"' während jedes Intervalls von dt Sekunden zwischen den
Abtastungen konstant ist. Bei einem bestimmten Wert von
y"' (t) können die Terme y" (t+dt), y' (t+dt) und y(t+dt)
durch Integration wie folgt gefunden werden:
-
Wenn (y", y',
y-Y) unter S liegt, wird der Term y"' gleich
+W gesetzt. Wenn y"' nach der Überschneidung mit der Fläche
auf -W geschaltet würde, träte ein Überschießen auf. Falls
y"' vor dem Überschneiden mit der Fläche auf -W
geschaltet würde, würde das Triple (y", y', y-Y) unter der Fläche
S und parallel dazu bleiben. Dieses anscheinende Dilemma
wird erfindungsgemäß überwunden durch ein Verfahren, das das
Auftreten von Oberflächendurchquerungen vorhersagt und
entsprechend den Wert von y"' einstellt.
-
Bei irgendeinem Abtastzeitpunkt dt nehmen wir an, daß wir
bereits y(t), y' (t) und y" (t) durch die Ausdrücke (31)
- (33) errechnet haben. Deswegen werden bei einem
Abtastzeitpunkt dt die Terme y' (t) und y" (t) benutzt, eine
Trajektorienüberschneidungsstelle (t) an der Zielfläche zu
berechnen. Wenn y(t) < (t), liegt die Trajektorie unter der
Fläche und y"' sollte +W sein, wenn kein Überschießen im
nächsten Intervall dt auftreten wird.
-
Die Wahrscheinlichkeit eines auftretenden Überschießens wird
bestimmt durch Errechnen von y(t+dt) und (t+dt) mit y"' =
+W. Falls y(t+dt) < (t+dt), wird kein Überschießen der
Fläche auftreten und der Term y"' (t) kann bei +W bleiben.
Falls y(t+dt) > (t+dt), steht ein Überschießen bevor. In
einem solchen Fall wird y"' (t) modifiziert und die Terme
y(t+dt), y'(t+dt) und y"(t+dt) werden neu berechnet.
Unglücklicherweise erfordert dieses Verfahren zwei
Berechnungen der Fläche S pro Zeitintervall, was eine zu große
Belastung für eine Steuereinheit mit begrenzter Rechenkapazität
ergeben kann.
-
Nach der dargestellten Ausführung werden vergangene Werte
von S und y benutzt, um zukünftige vorherzusagen. Unter
Benutzung von Näherungen zweiter Ordnung ergibt sich:
-
Mit beiden Ausführungen muß der Term y"' von +W
weggeschaltet werden, falls y(t+dt) ≥ S(t+dt), um ein Überschießen der
Fläche zu vermeiden. Der Term y"' (t) wird um eine hier
als DEL bezeichnete Größe zum Ausgleich des Positionsfehlers
es(t) modifiziert, wobei es(t) definiert wird entsprechend
dem Ausdruck:
-
Für Punkte unter der Fläche S kann y"' (t) deshalb definiert
werden als:
-
Der Term DEL wird so definiert, daß die Änderungsrate beim
Fehler es' (t) zwischen der Fläche S und der Trajektorie y
annähernd proportional zu es(t) ist mit einem konstanten
Porportionalitätsfaktor. Mit dieser Begrenzung fällt der Fehler
es(t) annähernd exponentiell ab, beginnend bei dem
Abtastintervall dt, bei welchem ein bevorstehendes Überschießen
erfaßt wird. Falls ts als ein Zeitraum zwischen
Abtastintervallen definiert wird - d.h. t ≤ t ≤ t+dt - kann die
Fehlercharakteristik algebraisch wie folgt ausgedrückt werden:
-
und durch Integration
-
wobei G die konstanten Rate ist. Projiziert über eine
ganzzahlige Anzahl n von Intervallen dt kann der Fehler
es(t+ndt) wie folgt ausgedrückt werden:
-
Anders gesagt, der Fehler es(t) fällt mit der exponentiellen
Rate (1 - G dt) ab.
-
Hält man das vorstehenden Ziel im Sinn, wird ein Ausdruck
für DEL abgeleitet durch Beziehen der Änderungsrate des
Fehlers es, (t) auf die Oberflächen- und Trajektorienterme. Es
folgt aus dem Ausdruck (35), daß
-
Für W(y' + y"²/2W) ≤ A² begegnet man den Geschwindigkeits-
und Beschleunigung-Begrenzungen nicht, und die
unmodifizierte Fläche S&supmin; des Ausdrucks (13) ist gültig.
Differenzieren des Ausdrucks (13) und Einsetzen von y"'(t) = (-W +
DEL) ergibt einen Ausdruck für S' (t) wie folgt:
-
Umordnen und Kombinieren der Ausdrücke (36) und (38) ergibt
einen Ausdruck für DEL:
-
In der Praxis haben wir gute Resultate mit (1 - G dt) = 1/2
erzielt. In anderen Worten, G wird so gewählt, daß das
Produkt G dt = 1/2. Funktionell bedeutet das, daß bei jedem
Intervall dt der Fehler es(t) mit einem Faktor zwei reduziert
wird.
-
Für W(y'+y"²/2W) ≥ A² muß die modifizierte Fläche S benutzt
werden. Unter Benutzen einer Ableitung analog zur vorstehend
beschriebenen kann der Term DEL wie folgt ausgedrückt
werden:
-
wobei x definiert ist als:
-
Zusammengefaßt: wenn y unter S beginnt und ein Überschießen
entweder bevorsteht oder aufgetreten ist, wird der Term y"'
von +W zu (-W + DEL) geschaltet, wobei der Term DEL durch
den Ausdruck (42) gegeben ist, wenn
-
und durch Ausdruck (43), wenn
-
In gleicher Weise kann, wenn y über der Fläche S gewesen
ist, wie vorstehend erklärt, eine Flächendurchquerung
zwischen den Zeitpunkten t und t+dt - d.h. y(t+dt) ≤ (t+dt)
- erwartet werden. Wenn eine bevorstehende Flächendurchquerung
gefunden wird, wird der Term y"' umgeschaltet von
-
Ausdrücke für den Term DEL werden in analoger Weise zu dem
vorstehend beschriebenen bezüglich der Trajektorien
abgeleitet, die anfangs über der Fläche liegen. Die Ausdrücke für
den Term DEL, bezogen auf Trajektorien, die anfangs unter
der Fläche liegen, für die Bedingungen W(y' - y"²/2W) ≤
-A² und W(y' - y"2/2W > -A² werden nachfolgend angegeben.
-
Ein gleichartiges Steuerproblem tritt auf bezüglich der
Kurve C, wenn die Trajektorie sich auf der Fläche
befindet. Wie bei der Annäherung an die Fläche ist die
Aufgabe, ein Überschießen der Kurve zu vermeiden. Die Lösung
für dieses Problem des Auftreffens auf die Kurve ist
gleichartig zu der des Auftreffens auf die Fläche
insoweit, als der Wert von y"' modifiziert wird, um einen Wert
zwischen +W und -W anzunehmen. Jedoch sind die Ausdrücke für
die Kurve viel leichter zu errechnen als die Ausdrücke für
die Fläche , wodurch es möglich ist, daß die Steuereinheit
22 eine exaktere Lösung benutzt.
-
Zunächst wird angenommen, daß die Trajektorie sich an der
Fläche &supmin; befindet und sich der Kurve &spplus; nähert. Es wird
weiter angenommen, daß W(y' + y"²/2W) ≤ A² -- d.h. die
Geschwindigkeits- und Beschleunigungs-Begrenzungen werden
nicht getroffen und der Ausdruck (9) für die unmodifizierte
Kurve C gilt.
-
Definiert man einen Term Q(t) als:
-
so folgt, daß Q(t) = 0, wenn die Trajektorie sich an der
Kurve C befindet. Auswerten des Ausdrucks Q(t+dt) schafft
eine Anzeige für ein bevorstehendes Überschießen. Falls die
Trajektorie sich an der Fläche &supmin; befindet und Q(t+dt)
kleiner als Null ist, wird ein bevorstehendes Überschießen
erfaßt. Falls Q(t+dt) größer als Null ist, wird vor dem
Zeitpunkt (t+dt) kein Überschießen auftreten.
-
Der Ausdruck für Q(t+dt) ist wie folgt:
-
Umgestellt ergibt sich
-
Es werde angenommen, daß bei einem bestimmten Intervall eine
Auswertung von Q(t+dt) anzeigt, daß ein Überschießen der
Kurve C bevorsteht mit y"'(t) = -W. Falls y"' unmittelbar
auf +W geschaltet wird, wird
-
was als positiv angenommen wird. Für y"' = -W
-
was als negativ angenommen wird. So muß für irgendeinen Wert
von y"' zwischen -W und +W gelten
-
Dieser Wert von y"' wird bei der dargestellten Ausführung
durch eine Sekanten-Näherung identifiziert, wobei Q als
eine Funktion von y"' betrachtet wird. Zwar sind andere
Verfahren verfügbar, einschließlich einer Newton'schen
Näherung oder einer direkten quadratischen Lösung, doch wird
die Sekanten-Näherung bevorzugt, da sie von Haus aus stabil
und bei Realzeitberechnungen wirtschaftlich ist.
-
Bei der Sekanten-Näherung wird P(w) definiert als
-
Die Sekanten-Näherung zu z = P(w) in dem Bereich -W ≤ w ≤
+W wird wie folgt ausgedrückt:
-
Eine Auflösung nach w bei z = 0 ergibt eine Näherung für
die Wurzel P(w) = 0:
-
Einsetzen von P(-W) und P(+W) in den Ausdruck (49) ergibt
den Wert w für y"' zwischen -W und W, der zur Kurve C
interpoliert:
-
Verbesserte Genauigkeit kann erzielt werden entweder durch
Wiederholen der Sekanten-Näherung unter Benutzen von (49)
als neuem Endpunkt oder durch Benutzen von
-
als einem Endpunkt der Interpolation. D.h., wenn P(0) > 0,
werden (P)0 und P(-W) benutzt, um die Sekante zu bestimmen;
wenn P(0) < 0, werden P(0) und P(+W) benutzt, die Sekante zu
bestimmen. Eine gleichartige Analyse ergibt analoge Formeln
zum Treffen auf &supmin; von &spplus;, wenn W(y'-y"²/2W) ≥ -A².
-
Die Annäherung an irgendeine Kurve im Phasenraum oder in der
(y', y")-Phasenebene kann in der gleichen Weise behandelt
werden. Wenn beispielsweise die Trajektorie mit W(y' +
y"²/2W) > A2 an &supmin; ist, dann ist der Abschnitt von &spplus;, der
getroffen wird, die Linie y" = -A. Das kann zum Zeitpunkt t
durch Prüfen der Ungleichung getestet werden
-
Falls der Ausdruck (51) erfüllt ist, muß y"' von -W auf
einen neuen Wert geschaltet werden. In diesem Fall ist keine
Interpolation notwendig und y"' wird entsprechend dem
Ausdruck bestimmt:
-
In darauffolgenden Intervallen wird y"' gleich Null
gesetzt, bis bestimmt ist, daß die Trajektorie die Kurve
C&spplus; mit y"' = 0 überschießt. Dann wird die Trajektorie zu der
C&spplus;-Kurve interpoliert unter Benutzung der vorstehend
beschriebenen Sekanten-Annäherung.
-
Sobald die Trajektorie an der C&spplus;-Kurve ist, muß das Ziel
y = Y im Ruhezustand getroffen werden. Infolge der
Annäherung nach Abtastzeitpunkten (digital) wird ein
Aufrechterhalten von y"' = +W mit aller Wahrscheinlichkeit nicht dazu
führen, daß die Trajektorie die Zielposition im Ruhezustand
trifft.
-
Entsprechend der dargestellten Ausführung dieser Erfindung
wird die End-Annäherungskurve C simuliert mit einer
kubischen Approximation, die es der Trajektorie erlaubt, die
Zielposition y = Y im Ruhezustand mit einem konstanten Wert
von y"' in einer ganzzahligen Anzahl N von Zeitschritten zu
treffen. Es sei angenommen, daß beim Auftreffen der
Trajektorie auf die Kurve C&spplus;, wie vorstehend beschrieben zum
Zeitpunkt t die Position y(t), die Geschwindigkeit y' (t)
und die Beschleunigung y"(t) ist. Für eine positive reale
Zahl x wird INT[x] als die größte ganze Zahl definiert, die
kleiner oder gleich x ist. Der Ausdruck N wird definiert wie
folgt:
-
Während der End-Annäherung an das Ziel y = Y ist die
Quantität x im Ausdruck (54) die dritte Ableitung des kubischen
Polynoms p(t), so daß
-
Verfolgung zu dem Ziel y = Y längs der End-Annäherungs-
Kubikfunktion
-
TEXT FEHLT
Anfangsbedingungen definiert. Die
Anfangs-Trajektorienkomponenten y(0)-Y, y'(0) und y"(0) werden dann benutzt, um zu
bestimmen, welche Flächenkomponente S&spplus; oder S&supmin; angemessen
ist; siehe Fig. 7.
-
Dann wird die angemessene Fläche S(0) oder (0) errechnet;
siehe Fig. 8 und 9. Der Term IBOUND wird benutzt, um
anzuzeigen, ob unmodifizierte oder modifizierte Flächen S,
bzw. Kurven C, angemessen sind. IBOUND wird auf 1 gesetzt,
wenn die unmodifizierte Fläche S und Kurve C verwendet
werden kann, und auf 2, wenn die modifizierte Fläche und
Kurve verwendet werden muß.
-
Dann wird die errechnete Fläche mit der
Anfangs-Trajektorienposition y(0)-Y verglichen, siehe Fig. 10. Wenn die
Trajektorie sich unter der Fläche befindet, wird ISTATE auf
1 gesetzt; wenn die Trajektorie über der Fläche ist, wird
ISTATE auf 2 gesetzt; und wenn die Trajektorie sich an der
Fläche befindet, wird ISTATE auf 7 gesetzt.
-
Dann bestimmt die Steuereinheit den richtigen Wert von y"'.
Falls ein Überschießen der Fläche bevorsteht, wird der
Term ISTATE auf 7 gesetzt und das Flußdiagramm der Fig. 17
ausgeführt, um y"' so zu modifizieren, daß sich die
Trajektorie exponentiell der Fläche nähert. Wenn die Fläche die
Geschwindigkeits-Begrenzung V getroffen hat, wird ISTATE
gleich 5 oder 6 gesetzt. Wenn die Trajektorie die
Beschleuigungs-Begrenzung A getroffen hat, wird ISTATE gleich 3
oder 4 gesetzt. Falls ein Überschießen einer der
Begrenzungen bevorsteht, interpoliert die Steuereinheit die
Begrenzungskurve durch Bestimmen eines angemessenen Wertes für
y"' zwischen -W und +W; siehe Fig. 11 oder 12. Danach
werden die Flußdiagramme der Fig. 21 oder 22 ausgeführt, um
die Trajektorienkomponenten y, y' und y" durch Integration
zu aktualisieren.
-
Beim nächsten Aufruf der Hilfsroutine MINTIME wird der
gegenwärtige Wert von ISTATE benutzt, um den Logikfluß in
die entsprechende Sequenz zu leiten. Wenn z.B. ISTATE = 3/4,
wird das Flußdiagramm der Fig. 13/14 ausgeführt, um eine
mögliche Flächenüberschießung zu prüfen. Wenn eine solche
erfaßt ist, wird ISTATE auf 7 gesetzt und y"' wie
vorstehend beschrieben modifiziert, so daß die Trajektorie sich
exponentiell der Fläche S nähert. Falls die
Geschwindigkeits-Begrenzung V getroffen wird, interpoliert die
Steuereinheit zu der Begrenzung und setzt ISTATE auf 5. Falls die
Geschwindigkeits-Begrenzung V nicht getroffen wurde, bleibt
ISTATE gleich 3 und y"' wird gleich 0 gesetzt. Danach
werden die Flußdiagramme der Fig. 21 und 22 ausgeführt, um
die Trajektorienkomponenten y, y' und y" durch Integration
wie vorher zu aktualisieren.
-
Falls ISTATE = 5/6, wird das Flußdiagramm nach Fig. 15/16
ausgeführt, um eine bevorstehende Flächenüberschießung zu
prüfen. Falls sie erfaßt ist, wird ISTATE wie vorher auf 7
gesetzt und y"' wird modifiziert, so daß sich die
Trajektorie exponentiell der Fläche S nähert. Falls nicht, und y" =
0, befindet sich die Trajektorie bei Spitzengeschwindigkeit.
In einem solchen Fall wird y"' (t) auf Null gesetzt und
y(t+dt) wird gleich y(t) + y'(t) dt gesetzt. Falls y"(t) -
W dt < 0, wird y' (t) gleich -y" (t) /dt gesetzt. Sonst gilt
y"' (t) = -W. In jedem Fall werden dann die Flußdiagramme
der Fig. 21 oder 22 ausgeführt, um die
Trajektorienkomponenten y, y' und y" wie vorher durch Integration zu
aktualisieren.
-
Falls ISTATE auf 7 gesetzt ist, werden die Flußdiagramme
nach Fig. 17-19 ausgeführt, um zu entscheiden, ob ein
Überschießen der Kurve C bevorsteht. Falls nicht, wird die
Linearkorrektur an y"' aufgrund von y(t) - Y - S(t)
fortgesetzt entsprechend den Ausdrücken (37) und (43) oder (44).
Falls ein Überschießen der Kurve C mit IBOUND = 2
bevorsteht, wird die Trajektorie zu der Kurve interpoliert und
die Steuerung setzt ISTATE auf 8 und y"' 0, bis IBOUND zu
1 zurückkehrt. Ist es einmal an der Kurve C, wird ISTATE
auf 9 gesetzt und die Steuereinheit errechnet die
endgültige Annäherungskubik entsprechend den Ausdrücken (55)
- (57). Wenn die Trajektorienposition y die Zielposition Y im
Ruhezustand trifft, wird ISTATE auf 10 gesetzt.
-
Die ISTATE-Variable kann für Steuerungszwecke ebenso
wertvoll sein, da sie das Fortschreiten des Weges anzeigt. Wenn
z.B. der Moment ISTATE = 7 erreicht wird, beginnt der Weg
abzubremsen, um Y zu treffen. Dieser Punkt könnte als ein
Signal für einen Zielwechsel gedeutet werden, falls ein sich
bewegendes Ziel das reale Zielobjekt ist. Ein anderer Fall
ist ISTATE = 8, was anzeigt, daß die Trajektorie sich auf
der abschließenden Annäherung an die Zielposition befindet.
In diesem Fall könnte die Steuereinheit einen
Parallelbetrieb einleiten, der mit dem Erreichen von y = Y in der
Ruhelage zu koordinieren ist.
-
Repräsentative durch die MINTIME-Routine erzeugte
Trajektorien werden graphisch in Fig. 24 - 28 dargestellt. Fig. 24
stellt die Monotonität der Trajektorie dar, die mit Ruhelage
beginnt und endet. Der Beschleunigungs-Begrenzung A war so
groß gewählt, daß der Wert nicht angewendet wird. In Fig. 25
werden sowohl der Beschleunigungs wie der Geschwindigkeits-
Grenzwert angewendet. In Fig. 26 - 28 ist die
Anfangsposition nicht in Ruhelage. In Fig. 28 sind die
Anfangs-Geschwindigkeit und -Beschleunigung so groß relativ zur
Anfangsposition, daß der Weg das Ziel (Y = 0) überschießen und
dann zurückkehren muß. Das ist die einzige Art des
Überschießens, die auftreten kann. In jedem Fall ist die
Erfassung einer bevorstehenden Überkreuzung angezeigt durch ein
"Abrunden" des idealen Rechteckwellen-Verhaltens von
y"'(t). Andere Abweichungen von dem idealen
Rechteckwellen-Verhalten von y"' ergeben sich infolge der kubischen
Approximation der End-Näherungskurve C.
Adaptive Vorwärtseinspeise-Wegverfolgung
-
Aus einer Anzahl von Gründen ist die Wegverfolgungs-Funktion
anhand der Motorspannungs-Steuerung dargestellt statt anhand
der Motorstrom-Steuerung Spannungssteuerung wird bevorzugt
(zumindest in Anwendungen mit variablen Lastdrehmoment-
Anforderungen), weil sie, anders als die Stromsteuerung,
relativ unempfindlich gegenüber Änderungen des
Lastdrehmoments ist. Da sie auf Änderungen der Motordrehzahl
empfindlich anspricht, kann leicht eine genaue Messung der
Motordrehzahl erhalten werden.
-
Durch die Unempfindlichkeit der Spannungssteuerung gegen
Lastdrehmoment haben wir gefunden, daß die Motordynamik
- nicht das Lastdrehmoment - das Verhalten der Bewegungs-
Steuervorrichtung beherrscht. Bei einem ankergesteuerten
Gleichstrom-Servomotor unter Trägheitsbelastung wie dem
Gleichstrom-Servomotor 12 in Fig. 1 kann eine solche
Dynamik algebraisch wie folgt ausgedrückt werden:
-
wobei
-
u = Motorspannung
-
R = Ankerwiderstand
-
L = Ankerinduktivität
-
J = Trägheit Motor plus Last
-
KT = Motordrehmoment-Konstante KT
-
B = viskose Dämpfungskonstant
-
Ke = Rück-EMK-Konstante
-
Tf = Drehmoment infolge statischer Reibung
-
x, x', x" und x"' = Motorposition,-Geschwindigkeit,
-Beschleunigung und -Stoß.
-
Es wird bemerkt, daß die Form des Ausdrucks (58) im
wesentlichen die gleiche wie für elektrische
Wechselstrom-Servomotoren und hydraulische Servomotoren ist.
-
Zur Bequemlichkeit wird Ausdruck (58) abgekürzt zu:
-
Die Eigenschaft der Ausdrücke (58) und (59), daß sie dritter
Ordnung sind, erfordert, daß auch die befohlene Trajektorie
dritter Ordnung ist. Mit anderen Worten, die dritte
Ableitung y"' der befohlenen Position y muß bestimmt sein. Nach
Definition erfüllt die durch die Wegerzeugungsfunktion
dieser Erfindung entwickelte Trajektorie dieses Kriterium.
-
Unter der Annahme eines Referenz- oder Befehlsweges y(t),
wie vorstehend definiert, ist der Spurfehler e(t) definiert
als:
-
Entsprechend der Wegfolgefunktion dieser Erfindung wird die
Referenz- oder Befehlstrajektorie verfolgt unter Benutzung
von Rückkoppel-Termen, die sich auf den Verfolgungsfehler
und seiner Änderungsrate gründen, von
Vorwärts-Einspeisetermen, die sich auf den Verfolgungsfehler und die
Trajektorienkomponenten y', y" und y"' und einen statischen
Reibungsterm Vf gründen. Algebraisch wird dies wie folgt
ausgedrückt:
-
Der Rückkoppel-Term ue(t) gründet sich auf den in Ausdruck
(60) definierten Verfolgungsfehler e(t) und kann auf jede
Weise bestimmt werden, welche die Fehlergleichung stabil
läßt:
-
Dies ist die Gleichung, die sich unter der Beeinflussung
durch den Ausdruck (61) mit der Annahme ergibt, daß Vf = Vf.
Bei der Ausführung dieser Erfindung wurde der Rückkoppelterm
ue(t) bestimmt unter Benutzung eines üblichen
Proportional/Differential-Verfahrens wie folgt:
-
wobei g&sub1; und g&sub2; auf die Stabilität des Ausdrucks (62) hin
ausgewählt sind. In einem solchen Fall konvergieren e"(t),
e'(t) und e(t) zusammen asymptotisch gegen Null und die
Motorposition x verfolgt die Referenztrajektorie in Hinsicht
auf Position y, Geschwindigkeit y' und Beschleunigung y".
-
Die Vorwärtseinspeise-Parameter v&sub1; - v&sub3; betreffen
physikalische Parameter des Motors, wie oben definiert, und sind
erfindungsgemäß in Realzeit abgestimmt, um den Ausdruck (61)
in Übereinstimmung mit dem physikalischen System zu bringen.
Wie später erklärt, sind die Vorwärtseinspeise-Parameter
individuell in Proportion zu dem Produkt aus Verfolgungsfehler
und dem jeweiligen Multiplikator des Ausdrucks (61)
eingestellt. Z.B. wird der Parameter v&sub1; eingestellt mit Bezug auf
das Produkt aus e(t) und y' (t). Algebraisch ist das:
-
wobei der Tiefindex "0" den gegenwärtigen Wert des Terms
bezeichnet und der Tiefindex "+1" den Wert des Terms für den
nächsten Zeitschritt (d.h. dt Sekunden später).
-
Der statische Reibungsterm Vf ist richtungsabhängig. Er hat
einen Wert +v&sub4;, falls y' > 0 und -v&sub5;, falls y' < 0. Die
Terme v&sub4; und v&sub5; werden in folgender Weise eingestellt:
-
Falls (y')0 > 0, dann (v&sub4;)&sbplus;&sub1; (v&sub4;)&sub0; + k&sub4; (e)&sub0;, und
(v&sub5;)&sbplus;&sub1; = (v&sub5;)&sub0;.
-
Falls (y')0 < 0, dann (v&sub4;)&sbplus;&sub1; = (v&sub4;)0, und
(v&sub5;)&sbplus;&sub1; = (v&sub5;)&sub0; - k&sub5; (e)&sub0;.
-
Falls (y')&sub0; = 0, bleiben v&sub4; und v&sub5; ungeändert.
-
In der vorher beschriebenen Weise sichern die
Vorwärtseinspeise-Terme einen Spurfehler von im wesentlichen Null
innerhalb der dynamischen Fähigkeit des
Gleichstrom-Servomotors 12 durch Vorwegnahme der Dynamik der Trajektorie.
Daraus ergibt sich, daß die Dynamik der Trajektorie
unmittelbar in der Steuerspannung u(t) des
Gleichstrom-Servomotors wiedergespiegelt wird. Die Koeffizienten v&sub1; - v&sub5; werden
eindeutig durch die physikalischen Merkmale des
Gleichstrom-Servomotors 12 und der Last bestimmt, wie auch die
Abtastrate der Steuerung und der (Rückkoppel-)Regelung. Änderungen
der Merkmale von Motor zu Motor und von Tag zu Tag und damit
der Koeffizienten werden ausgeglichen durch adaptives
Nachstellen, da das Nachstellen auf dem Spurfehler und den
Befehlsabgeleiteten beruht. Als Ergebnis kann die Bewegungs-
Steuervorrichtung die Trajektorie dritter Ordnung virtuell
mit Fehler Null verfolgen.
-
Ein die Wegfolgefunktion im Steuervorrichtungsformat
darstellendes Diagramm ist in Fig. 29 abgebildet. In diesem
Diagramm sorgt die Wegerzeugungsfunktion dieser Erfindung
für die y"'-Begrenzung, aus der y", y' und y durch
sukzessive Integration errechnet werden. Der Folgefehler e(t) wird
bestimmt entsprechend der Differenz zwischen der Wegposition
y und der rückgemeldeten Motorposition x und wird benutzt,
um die verschiedenen Terme v&sub1; - v&sub5; zu berechnen. Die
Rückkopplungs-, statischen Reibungs- und
Vorwärtseinspeise-Spannungen werden summiert zur Bildung einer Steuerspannung V,
die an den Gleichspannungsbetrieb-Leistungsverstärker 16 zur
entsprechenden Beaufschlagung des
Gleichspannungs-Servomotors 12 angelegt wird.
-
Ein Flußdiagramm zum Ausführen der
Bewegungssteuer-Funktionen dieser Erfindung wird in Fig. 30 gegeben. Der Block 100
in Fig. 30 bezeichnet allgemein eine Reihe von
Programminstruktionen, die bei der Initialisierung jedes
Zeitabschnitts des Servobetriebs ausgeführt werden zum
Initialisieren der Werte der verschiedenen Terme und Register.
Beispielsweise wird der Term ISTATE der Wegerzeugungs-Routine
auf Null gesetzt und die Vorwärtseinspeise-Parameter v&sub1; - v&sub5;
werden auf eine vorbestimmte Schätzung ihres wahren Wertes
gesetzt oder auf Werte, die in einer vorhergehenden
Betriebszeit "gelernt" wurden.
-
Nach der Initialisierung der verschiedenen Terme und
Register der Steuereinheit 22 wird ein Realzeittakt gestartet,
wie im Schritt 102 gezeigt, und die Instruktionsschritte 104
- 120 werden wiederholt der Reihe nach ausgeführt, wie durch
die Flußdiagramm-Linie 122 dargestellt. Die Schritte 108
- 120 arbeiten in Verbindung mit dem Realzeittakt und einem
Interrupt-Zähler (INT COUNTER), um die Ausführungsrate der
Schritte 104 - 116 festzusetzen.
-
Die hier beschriebenen Minimalzeit-Wegerzeugungssteuerung
und adaptive Vorwärtseinspeise-Verfolgungsroutine wurden
zusammen an einem Motorola 68000-Mikroprozessor mit
Fließpunkt-Festware ausgeführt. Die Aufgaben minimale Zeit zum
Erreichen des Ziels und im wesentlichen kein Überschießen
wurden mit einer Hauptschleifen-Zykluszeit von ca. 5,25 ms
erreicht.
-
Fig. 31 und 32 stellen graphisch die Verbesserungen dar, die
bei einer Anwendung der vorstehenden Mechanisierung auf den
Gürtelachsen-Servomechanismus eines üblichen
Industrieroboters beobachtet wurde. Jede Figur stellt den Betrieb des
Servomechanismus bei voller Nenngeschwindigkeit einerseits
mit einer konventionellen Bewegungssteuervorrichtung und
andererseits mit der vorliegenden Bewegungssteuervorrichtung
dar.
-
Fig. 31 zeigt die tatsächliche Beschleunigung des
Servomechanismus unter Benutzung von gleichen
Beschleunigungs-Skalen und einer gemeinsamen Zeitbasis. Die übliche Steuerung
(Graphik a) ist zweiter Ordnung, und Befehle ändern die
Beschleunigung stufenweise, wie durch die gestrichelte Spur
angezeigt. Wenn die Beschleunigung zum Zeitpunkt t1 befohlen
wird, kann der Servomechanismus die Schrittänderung nicht
erreichen und es besteht ein großer Anfangsfehler, gefolgt
von einem Überschießen und gedämpfter Schwingung. Die
gleichen Phänomene treten auf, wenn zum Zeitpunkt t2 Abbremsung
befohlen wird. In der Praxis ergibt das eine Unstetigkeit
und ein Überschießen der Zielposition. Die erfindungsgemäße
Steuerung (Graphik b) ist dritter Ordnung und ergibt einen
verfolgbaren Beschleunigungsbefehl. Die Beschleunigung des
Servomechanismus folgt dem Befehlswert, ohne die
Zielposition wesentlich zu überschießen. Die Motorspannungs- und
Motorstrom-Aufzeichnungen (nicht dargestellt) haben ähnliche
Profile.
-
Fig. 32 stellt die Wegende-Oszillation des Servomechanismus
dar, gemessen an der Spitze einer elastischen Servolast
(Endeffektor). Eine derartige Oszillation entspricht der
Abklingzeit der Last, sobald die Zielposition erreicht wurde.
Mit der üblichen Steuerung (Graphik a) ist eine
ausgesprochene Schwingung vorhanden, die nach ca. 6 Sekunden auf
einen relativ niedrigen Pegel abgedämpft wird. Die mit der
Bewegungs-Steuerungsvorrichtung dieser Erfindung erreichte
raschere Abklingzeit läßt eine erhöhte Zykluszahl bei
Anwendungen wie Punktschweißen zu, bei denen keine erhebliche
Lastschwingung erlaubt ist. Darüberhinaus ergibt die
herabgesetzte Anzahl und Amplitude der damit verbundenen
Drehmoment-Umkehrungen eine verbesserte Lebensdauer des
Servomechanismus.
-
Diese Erfindung wurde zwar in Bezug auf die dargestellte
Ausführung beschrieben, es ist jedoch gut zu verstehen, daß
verschiedene Abwandlungen dem Fachmann auf diesem Gebiet
möglich erscheinen. Beispielsweise ist die
Wegerzeugungsfunktion dieser Erfindung gleich gut auch auf hydraulische
Servovorrichtungen oder auf elektrische Servovorrichtungen
mit einem Wechselstrommotor anwendbar.