Titel

Verfahren zur Arretierung eines Werkzeugs einer Baumaschine in einer vorgegebenen Neigung

Die vorliegende Erfindung betrifft ein Verfahren zur Arretierung eines Werkzeugs einer Baumaschine in einer vorgegebenen Neigung mittels einer Tool Center Point Estimation. Ferner betrifft die Erfindung ein Computerprogramm, das jeden Schritt des Verfahrens ausführt, wenn es auf einem Rechengerät abläuft, sowie ein maschinenlesbares Speichermedium, welches das Computerprogramm speichert. Schließlich betrifft die Erfindung ein elektronisches Steuergerät, welches eingerichtet ist, das erfindungsgemäße Verfahren auszuführen.

Stand der Technik

Bei vielen Baumaschinen, bei denen das Werkzeug über einen Arbeitsarm mit der Baumaschine verbunden ist, ist es wichtig, das Werkzeug mit einer bestimmten Neigung zu führen. Vor allem bei Baumaschinen, bei denen das Werkzeug eine Schaufel ist, wie z. B. Radlager, Bagger oder Ähnliche, ist die richtige Einstellung der Neigung der Schaufel wichtig, um zu verhindern, dass die Ladung der Schaufel verschüttet wird. Beim Anheben von Ladung kann es aufgrund der Kinematik zu einem Verkippen der Schaufel kommen. Zudem kann sich die Baumaschine mitsamt der Schaufel beim Fahren auf unebenem

Untergrund so stellen, dass die Ladung aus der Schaufel fällt. Dies muss normalerweise von einem Bediener korrigiert werden. Auch bei anderen Typen von Baumaschinen kann es wichtig sein, die Neigung des Werkzeugs richtig einzustellen. Gerade für ungeübte Bediener ist die ständige Überwachung der Neigung des Werkzeugs oft schwierig. Außerdem ist die richtige Neigung des Werkzeugs für einen autonomen Betrieb der Baumaschine wichtig.

Es sind Algorithmen zur Bestimmung der kinematischen Kette bekannt. An jedem Glied des Werkzeugarms ist hierfür einer oder mehrere der folgenden Sensoren inertiale Messeinheit (IMU, inertial measuring unit), Winkelsensoren, Linearsensoren angeordnet, welche Sensordaten an ein Rechengerät senden. Die so ermittelten Sensordaten werden für jeden Sensor individuell gefiltert und zur Zustandsschätzung der Orientierung des jeweiligen Sensors relativ zu einem ortsfesten Inertialkoordinatensystem fusioniert. Die Tool Center Point Estimation ist ein Algorithmus zur Zustandsschätzung der Orientierung und Position eines Endeffektors. Der Endeffektor ist insbesondere ein Werkzeug oder ein Teil eines Werkzeugs, das einen Werkzeugarm mit mehreren Gliedern, die über Gelenke verbunden sind, aufweist.

Typischerweise verwendete Verfahren sind in der Abhandlung von Nikolas Trawny und Stergios I. Roumeliotis. "Indirect Kalman filter for 3D attitude estimation" University of Minnesota, Dept. of Comp. Sei. & Eng., Tech. Rep 2 (2005), in der Abhandlung von Robert Mahony, Tarek Hamei, und Jean-Michel Pflimlin, "Nonlinear complementary filters on the special orthogonal group", IEEE Transactions on automatic control 53.5 (2008): 1203-1218, sowie in der

Abhandlung von Sebastian Madgwick, "An efficient Orientation filter for inertial and inertial/magnetic sensor arrays" Report x-io and University of Bristol (UK) 25 (2010), beschrieben, auf die insoweit verwiesen wird.

Aus der so geschätzten Orientierung des Sensors wird zunächst die Orientierung des Glieds, an dem der Sensor angeordnet ist, bestimmt. Dies wird für alle Glieder des Werkzeugarms durchgeführt. Aus der relativen Orientierung zweier aufeinanderfolgender Glieder lässt sich bei bekannter Kinematik (zum Beispiel bei bekannten Denavit-Hartenberg Parametern) der Gelenkwinkel des Gelenks, das die beiden Glieder verbindet, berechnen. Sind schließlich alle Gelenkwinkel und die Maße der Glieder bekannt, folgt die gesamte Konfiguration des

Werkzeugarms direkt aus der Vorwärtskinematik und somit die Orientierung und Position des Endeffektors.

Für eine detaillierte Beschreibung wird auf die Abhandlung von Mark W. Spong, Seth Hutchinson und Mathukumalli Vidyasagar,„Robot modeling and control”,

Vol. 3. New York: Wiley, 2006, verwiesen.

Offenbarung der Erfindung

Es wird ein Verfahren zur Arretierung eines Werkzeugs einer Baumaschine in einer vorgegebenen Neigung vorgeschlagen. Während des gesamten Verfahrens wird die Neigung des Werkzeugs mit Hilfe eines oder mehrerer der folgenden Sensoren inertiale Messeinheit, Winkelsensoren, Linearsensoren bestimmt.

Rein prinzipiell kann das Werkzeug direkt über ein Gelenk mit der Baumaschine verbunden sein. Vorzugsweise ist allerdings ein Arbeitsarm vorgesehen, der auf der einen Seite über ein Gelenk mit der Baumaschine verbunden ist und auf der anderen Seite ebenfalls über ein Gelenk mit dem Werkzeug verbunden ist.

Bevorzugt ist der Arbeitsarm mehrgliedrig ausgebildet, wobei an jedem Glied des Arbeitsarms einer der obengenannten Sensoren angeordnet ist. Aus der Position und der Neigung bzw. der Orientierung des Werkzeugs kann aus der Tool Center Point Estimation auf die Gelenkwinkel der beschriebenen Gelenke geschlossen werden.

Wenn die Baumaschine, eine Komponente der Baumaschine und/oder eine mit der Baumaschine und dem Werkzeug verbundene Komponente bewegt wird, erfolgt eine Regelung zumindest eines Gelenkwinkels des Werkzeugs auf Basis der ermittelten Neigung des Werkzeugs. Vorzugsweise ist die mit der

Baumaschine und dem Werkzeug verbundene Komponente der oben

beschriebene Arbeitsarm mit den Gelenken. Für diesen Fall werden der

Gelenkwinkel des Gelenks zwischen Werkzeug und Arbeitsarm und/oder der Gelenkwinkel das Gelenk zwischen Arbeitsarm und Baumaschine auf Basis der ermittelten Neigung des Werkzeugs geregelt. Die Regelung erfolgt derart, dass die Neigung des Werkzeugs bezüglich einer vorgebbaren Referenz konstant gehalten wird. Damit wird eine Arretierung des Werkzeugs erreicht. Als Resultat können Unzulänglichkeiten der Kinematik ausgeglichen werden und zwar nicht mechanisch, sondern elektronisch über die Regelung. Dadurch wird ein Bediener der Baumaschine entlastet, da er die Neigung des Werkzeugs nicht dauerhaft überwachen muss. Außerdem kann eine autonom betriebene Baumaschine auf dieses Verfahren zurückgreifen, um die Neigung des Werkzeugs konstant zu halten.

Gemäß einem Aspekt ist die vorgebbare Referenz ein Teil der Baumaschine. Vorteilhafterweise kann aus der Referenz auf den Abstand zum Boden und auf die Neigung des Bodens geschlossen werden. Hierfür kann der Teil der

Baumaschine einen inertialen Sensor aufweisen. Insbesondere kann die

Referenz ein Gehäuse der Baumaschine oder ein Teil davon sein. Wenn das Werkzeug und/oder der Arbeitsarm bewegt werden, wird wenigstens ein

Gelenkwinkel zumindest eines der Gelenke zwischen dem Werkzeug und der Baumaschine so geregelt, dass die Neigung des Werkzeugs in einem konstanten Winkel zu dem Teil der Baumaschine gehalten wird. Für den Fall, dass der Arbeitsarm über ein erstes Gelenk mit der Baumaschine verbunden ist und über ein zweites Gelenk mit dem Werkzeug verbunden ist und gegebenenfalls weitere Gelenke zwischen seinen Gliedern aufweist, wird vorzugsweise der Gelenkwinkel des Gelenks zwischen dem Werkzeug und dem Arbeitsarm so geregelt, dass die Neigung des Werkzeugs in einem konstanten Winkel zu dem Teil der

Baumaschine gehalten wird. Zum Beispiel, wenn das Werkzeug vertikal bewegt wird, indem der Arbeitsarm geschwenkt wird, wird der sich durch das Schwenken verändernde Winkel zwischen dem Arbeitsarm und dem Teil der Baumaschine durch die Regelung des Gelenkwinkels des Gelenks zwischen dem Werkzeug und dem Arbeitsarm ausgeglichen. Mit anderen Worten wird das Verkippen der Schaufel durch eine entgegengesetzte Verkippung ausgeglichen.

Zur Bestimmung der Neigung und der Position des Werkzeugs kann ein

Algorithmus zur Bestimmung der kinematischen Kette verwendet werden. Hierfür sind die obengenannten Sensoren an jedem Glied der kinematischen Kette zwischen der Baumaschine und dem Werkzeug angeordnet. Insbesondere kann die Tool Center Point Estimation zur Bestimmung der Neigung und der Position des Werkzeugs verwendet werden. Diese verwendet bevorzugt Sensorsignale von inertialen Messeinheiten, die inertiale Sensoren aufweisen, die an jedem Glied der kinematischen Kette angeordnet sind. Inertiale Messeinheiten lassen sich leicht und kostengünstig nachrüsten und können für andere Verfahren verwendet werden.

Gemäß einem weiteren Aspekt ist die vorgebbare Referenz eine Richtung der Erdgravitation. Vorteilhafterweise kann die Bestimmung der Neigung des

Werkzeugs gegenüber der Erdgravitation mit Hilfe eines inertialen Sensors am Werkzeug erfolgen. Wenn sich die Neigung des Werkzeugs gegenüber der Erdgravitation ändert, insbesondere wenn sich die Neigung der ganzen

Baumaschine gegenüber der Erdgravitation ändert, wird zumindest ein

Gelenkwinkel eines der Gelenke zwischen dem Werkzeug und der Baumaschine so geregelt, dass die ursprüngliche Neigung des Werkzeugs gegenüber der Erdgravitation wiederhergestellt wird. Für den Fall, dass der Arbeitsarm über ein erstes Gelenk mit der Baumaschine verbunden ist und über ein zweites Gelenk mit dem Werkzeug verbunden ist und gegebenenfalls weitere Gelenke zwischen seinen Gliedern aufweist, wird vorzugsweise der Gelenkwinkel des ersten Gelenks so geregelt, dass die ursprüngliche Neigung des Werkzeugs gegenüber der Erdgravitation wiederhergestellt wird. Zum Beispiel, wenn die Baumaschine auf unebenem Gelände fährt und dabei auf eine Erhöhung oder in eine

Vertiefung fährt, neigt sich die Baumaschine gemäß dem Gefälle und der verändernde Winkel zwischen der Baumaschine und der Erdgravitation wird durch die Regelung des Gelenkwinkels des Gelenks zwischen dem Arbeitsarm und der Baumaschine ausgeglichen. Mit anderen Worten wird das Verkippen der Baumaschine durch eine entgegengesetzte Verkippung des Arbeitsarms ausgeglichen. Auch das oben beschriebene Beispiel, dass das Werkzeug vertikal bewegt wird, kann bei der Regelung mit der Erdgravitation als Referenz in gleicher Weise realisiert werden.

Wenn sich die Baumaschine bewegt, kann eine Beschleunigung, die bei der Bewegung der Baumaschine auftritt, durch den inertialen Sensor registriert werden und bei beim Regeln des Gelenkwinkels des Werkzeugs kompensiert werden, sodass die Resultierende aus der Erdbeschleunigung und der

Bewegungsbeschleunigung in die gewünschte Richtung, z. B. zum Boden des Werkzeugs, zeigt.

Alternativ kann auch hier zur Bestimmung der Neigung des Werkzeugs gegenüber der Erdgravitation ein Algorithmus zur Bestimmung der kinematischen Kette, wie oben bereits beschrieben, verwendet werden.

Das Verfahren kann beispielsweise durch einen Bediener über ein

Bedienelement, wie z. B. ein Schalter, ein Knopf, ein Touchscreen oder

Ähnliches, eingegeben werden. Wenn das Werkzeug arretiert ist, kann der Bediener vorteilhafterweise den Gelenkwinkel zwischen dem Werkzeug und dem Arbeitsarm oder der Baumaschine nicht mehr eigenständig steuern.

Das Computerprogramm ist eingerichtet, jeden Schritt des Verfahrens

durchzuführen, insbesondere, wenn es auf einem Rechengerät oder Steuergerät durchgeführt wird. Es ermöglicht die Implementierung des Verfahrens in einem herkömmlichen elektronischen Steuergerät, ohne hieran bauliche Veränderungen vornehmen zu müssen. Hierzu ist es auf dem maschinenlesbaren

Speichermedium gespeichert. Durch Aufspielen des Computerprogramms auf ein herkömmliches

elektronisches Steuergerät, wird das elektronische Steuergerät erhalten, welches eingerichtet ist, die Arretierung des Werkzeugs durchzuführen.

Kurze Beschreibung der Zeichnungen

Ausführungsbeispiele der Erfindung sind in den Zeichnungen dargestellt und in der nachfolgenden Beschreibung näher erläutert.

Figur 1 zeigt eine schematische Darstellung einer Baumaschine, bei der ein Werkzeug aus einem Anfangszustand (a) in einen Endzustand (b) bewegt wird.

Figur 2 zeigt ein Ablaufdiagramm eines Ausführungsbeispiels des

erfindungsgemäßen Verfahrens.

Figur 3 zeigt eine schematische Darstellung einer Baumaschine, bei der die Baumaschine von einem ebenen Boden (a) auf eine Erhöhung (b) fährt.

Figur 4 zeigt ein Ablaufdiagramm eines weiteren Ausführungsbeispiels des erfindungsgemäßen Verfahrens.

Ausführungsbeispiele der Erfindung

Figur 1 zeigt eine schematische Darstellung einer Baumaschine 1 in Form eines Radladers mit einem als Schaufel ausgebildeten Werkzeug 2. Das Werkzeug 2 ist über einen Arbeitsarm 3 mit der Baumaschine 1 verbunden, wobei zwischen der Baumaschine 1 und dem Arbeitsarm 3 ein erstes Gelenk 4 und zwischen dem Werkzeug 2 und dem Arbeitsarm 3 ein zweites Gelenk 5 angeordnet sind, welche die jeweiligen Komponenten beweglich verbinden. In weiteren nicht gezeigten Ausführungsbeispielen kann der Arbeitsarm auch mehrgliedrig ausgebildet sein, wobei in diesem Fall zwischen den einzelnen Gliedern ebenfalls jeweils ein Gelenk angeordnet ist. Die Baumaschine 1, der Arbeitsarm 3 und das Werkzeug 2 bilden eine kinematische Kette. An jedem Glied der kinematischen Kette, also an der Baumaschine 1, dem Arbeitsarm 3 und dem Werkzeug 2, ist jeweils ein inertialer Sensor 6 einer inertialen Messeinheit angeordnet. Dem inertialen Sensor 6‘ am Werkzeug 2 kommt hierbei eine besondere Bedeutung zu (siehe unten), er ist daher mit einem Strich (‘) gekennzeichnet. Die inertialen Sensoren 6 sind mit einem elektronischen

Steuergerät 7 der Baumaschine 1 verbunden.

In Figur 1 sind zwei Zustände a) und b) dargestellt, die zu unterschiedlichen Zeiten aufgenommen wurden. Das Werkzeug 2 ist im Anfangszustand a) auf einem niedrigen Level und wird bis zum Endzustand b) auf ein höheres Level angehoben. Das Anheben des Werkzeugs erfolgt in diesem Fall, indem der Gelenkwinkel des ersten Gelenks 4 zwischen dem Arbeitsarm 3 und der

Baumaschine 1 verändert wird.

Ohne Eingriff oder Regelung (hier nicht dargestellt) würde das zweite Gelenk 5 in beiden Zuständen bezüglich dem Arbeitsarm 3 gleich ausgerichtet sein, d. h. der Gelenkwinkel des zweiten Gelenks 5 zwischen dem Werkzeug 2 und dem Arbeitsarm 3 würde im Anfangszustand a) und im Endzustand b) gleich sein, obwohl sich der Gelenkwinkel des ersten Gelenks 4 verändert hat. Dies würde zur Folge haben, dass das Werkzeug 2, d. h. die Schaufel, geneigt wäre und deren Inhalt verschüttet würde.

Mittels dem erfindungsgemäßen Verfahren wird während des Anhebens aus dem Anfangszustand a) in den Endzustand b) der Gelenkwinkel des zweiten Gelenks 5 geregelt und zwar derart, dass die sich durch das Anheben verändernde Neigung des Werkzeugs 2 ausgeglichen wird, sodass das Werkzeug 2 im

Endzustand b), wie bereits im Anfangszustand a), senkrecht zur Hochachse (nicht dargestellt) der Baumaschine 1 steht. In diesem Beispiel wird die Änderung des Gelenkwinkels des ersten Gelenks 4 durch eine gleich große Änderung des Gelenkswinkels des zweiten Gelenks 5 in die entgegengesetzte Drehrichtung ausgeglichen.

In Figur 2 ist ein Ablaufdiagramm eines Ausführungsbeispiels des

erfindungsgemäßen Verfahrens dargestellt. Zu Beginn und während des ganzen Verfahrens wird die aktuelle Neigung und die Position des Werkzeugs 2 unter Zuhilfenahme einer an sich bekannten Tool Center Point Estimation bestimmt 10. Hierfür werden die Sensordaten der inertialen Sensoren 6, 6‘ entlang der kinematischen Kette verwendet. Aus der Neigung und der Position des

Werkzeugs 2 und des Arbeitsarms 3 wird dann mittels sogenannter Denavit- Hartenberg Parametern (siehe beispielsweise Spong et al.„Robot modeling and control”, Vol. 3. New York: Wiley, 2006) ein aktueller Ist-Gelenkwinkel D,st zumindest für das zweite Gelenk 5 ermittelt 11.

Wird die Arretierung des Werkzeugs 2 durch einen Bediener aktiviert 20, beispielsweise indem er einen dafür vorgesehenen Knopf betätigt, so wird die aktuelle Neigung des Werkzeugs 2 bezüglich der Hochachse der Baumaschine 1 als vorgegebene Soll-Neigung festgelegt 30. Dann wird ein Soll-Gelenkwinkel D _SO II zumindest für das zweite Gelenk 5 ermittelt 31, bei dem das Werkzeug 2 die Soll- Neigung beibehält. Schließlich ist eine Regelung 40 zur Arretierung des

Werkzeugs 2 vorgesehen, bei der, während beispielsweise der Arbeitsarm 3 bewegt wird und sich der Gelenkwinkel des ersten Gelenks 4 ändert, der Ist- Gelenkwinkel Dist auf den Soll-Gelenkwinkel D _SO II geregelt wird, sodass die Neigung des Werkzeugs 2 bezüglich der Hochachse der Baumaschine 1 konstant gehalten wird.

Figur 3 zeigt ebenfalls eine schematische Darstellung einer Baumaschine 1 wie aus Figur 1. Gleiche Komponenten sind mit gleichen Bezugszeichen

gekennzeichnet und es wird auf deren Beschreibung in Bezug auf Figur 1 verwiesen. In Figur 3 sind ebenfalls zwei Zustände a) und b) dargestellt, die zu unterschiedlichen Zeiten aufgenommen wurden. Die Baumaschine 1 steht im Anfangszustand a) auf ebenem Boden 8 und fährt im Endzustand b) auf eine Erhöhung 9. Anstelle der Erhöhung 9 könnte auch eine Vertiefung vorhanden sein. Zudem sind die Richtung der Erdgravitation G und eine dazu orthogonale Richtung gekennzeichnet. Durch das Fahren auf die Erhöhung 9 wird die

Baumaschine 1 bezüglich der Richtung der Erdgravitation G geneigt.

Ohne Eingriff oder Regelung (hier nicht dargestellt) würde das erste Gelenk 4 und das zweite Gelenk 5 in beiden Zuständen bezüglich der Baumaschine 1 gleich ausgerichtet sein, d. h. der Gelenkwinkel des zweiten Gelenks 5 zwischen dem Werkzeug 2 und dem Arbeitsarm 3 und der Gelenkwinkel des ersten Gelenks 4 zwischen dem Arbeitsarm 3 und der Baumaschine 1 würden im Anfangszustand a) und im Endzustand b) gleich sein, obwohl sich die Neigung der Baumaschine 1 verändert hat. Dies würde die Folge haben, dass das Werkzeug 2, d. h. die Schaufel, in Bezug auf die Erdgravitation geneigt wäre und deren Inhalt verschüttet würde. Mittels des erfindungsgemäßen Verfahrens wird, während sich die Baumaschine 1 aus dem Anfangszustand a) in den Endzustand b) bewegt, der Gelenkwinkel des ersten Gelenks 4 geregelt und zwar derart, dass die sich verändernde Neigung des Werkzeugs 2 und des Arbeitsarms 3 ausgeglichen wird, sodass das Werkzeug 2 im Endzustand b), wie bereits im Anfangszustand a), senkrecht zur Richtung der Erdgravitation steht. In diesem Beispiel wird die Änderung der Neigung der Baumaschine 1 durch eine gleich große Änderung des

Gelenkswinkels des ersten Gelenks 4 in die entgegengesetzte Drehrichtung ausgeglichen. In weiteren Ausführungsbeispielen wird alternativ oder zusätzlich, während sich die Baumaschine 1 aus dem Anfangszustand a) in den Endzustand b) bewegt, der Gelenkwinkel des ersten Gelenks 4 geregelt und zwar derart, dass die sich verändernde Neigung des Werkzeugs 2 ausgeglichen wird, sodass das Werkzeug 2 im Endzustand b), wie auch im Anfangszustand a), senkrecht zur Richtung der Erdgravitation G steht. Die Änderung der Neigung der

Baumaschine 1 wird demnach zusätzlich oder alternativ durch eine Änderung des Gelenkswinkels des zweiten Gelenks 5 ausgeglichen.

In Figur 4 ist ein Ablaufdiagramm eines weiteren Ausführungsbeispiels des erfindungsgemäßen Verfahrens dargestellt. Zu Beginn und während des ganzen Verfahrens wird die aktuelle Neigung des Werkzeugs 2 bezüglich der

Erdgravitation G mittels des inertialen Sensors 6‘ am Werkzeug 2 bestimmt 110. Alternativ kann, wie im oberen Ausführungsbeispiel beschrieben, die Neigung des Werkzeugs 2 bezüglich der Erdgravitation G unter Zuhilfenahme der Tool Center Point Estimation bestimmt werden, wobei hierfür die Sensordaten der inertialen Sensoren 6 entlang der kinematischen Kette verwendet werden.

Anhand der Sensordaten der inertialen Sensoren 6 kann mittels sogenannter Denavit-Hartenberg Parameter (siehe beispielsweise Spong et al.„Robot modeling and control”, Vol. 3. New York: Wiley, 2006) ein aktueller Ist- Gelenkwinkel Di _st zumindest für das erste Gelenk 4 und zusätzlich oder alternativ für das zweite Gelenk 5 und darauf basierend die Neigung des Werkzeugs (2) ermittelt 111 werden.

Wenn sich das Fahrzeug von aus dem Anfangszustand a) in den Endzustand b) bewegt, tritt eine Beschleunigung auf. Diese Bewegungsbeschleunigung der Baumaschine 1 wird durch den inertialen Sensor 6‘ registriert. Wird die Arretierung des Werkzeugs 2 durch einen Bediener aktiviert 120, beispielsweise indem er einen dafür vorgesehenen Knopf betätigt, so wird die aktuelle Neigung des Werkzeugs 2 bezüglich der Erdgravitation als vorgegebene Soll- Neigung festgelegt 130. Dann wird ein Soll-Gelenkwinkel D _SO II zumindest für das erste Gelenk 4 und zusätzlich oder alternativ für das zweite Gelenk 5 ermittelt 31, bei dem das Werkzeug 2 die Soll-Neigung beibehält. Schließlich ist eine Regelung 140 zur Arretierung des Werkzeugs 2 vorgesehen, bei der, während beispielsweise die Baumaschine 1 bewegt wird und sich neigt, die Ist- Gelenkwinkel Di _st auf die Soll-Gelenkwinkel D _SO II geregelt werden, sodass die Neigung des Werkzeugs 2 bezüglich der Erdgravitation G konstant gehalten wird.

Die bei der Bewegung der Baumaschine 1 auftretende

Bewegungsbeschleunigung (siehe oben) wird bei der Regelung 140 kompensiert, sodass die Resultierende aus der Erdbeschleunigung und der

Bewegungsbeschleunigung zum Boden des Werkzeugs 2 hin zeigt.