DE102019201091A1

DE102019201091A1 - Verfahren zur Zustandsschätzung von Lage und Orientierung von mehreren beweglichen Modulen eines gemeinsamen Systems

Info

Publication number: DE102019201091A1
Application number: DE102019201091.1A
Authority: DE
Inventors: Alejandro Lopez Pamplona; Sebastian Scherer; Daniel Neyer
Original assignee: Robert Bosch GmbH
Current assignee: Robert Bosch GmbH
Priority date: 2019-01-29
Filing date: 2019-01-29
Publication date: 2020-07-30
Also published as: JP2022523713A; US20220162834A1; CN113330167B; CN113330167A; JP7250940B2; WO2020156897A1; EP3918138A1

Abstract

Die Erfindung betrifft ein Verfahren zur Zustandsschätzung von Lage und Orientierung von mehreren zueinander über Gelenke (J1, J2) beweglichen Modulen eines gemeinsamen Systems mittels Inertialsensoren (S1, S2), die an den Modulen angeordnet sind. Dabei wird zumindest ein Vektorpaar (n,nodero,o) ermittelt (100, 101, 110, 111), welches kinematische Zusammenhänge zumindest eines der Gelenke (J1, J2) und der beiden mit dem Gelenk (J1, J2) verbundenen Module repräsentiert, und welches in die Zustandsschätzung einfließt.

Description

Die vorliegende Erfindung betrifft ein Verfahren zur Zustandsschätzung von Lage und Orientierung von mehreren zueinander über Gelenke beweglichen Modulen eines gemeinsamen Systems, bei dem kinematische Zusammenhänge der beweglichen Module und der Gelenke in die Zustandsschätzung einfließen. 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
Heutzutage schreitet die Automatisierung im Bereich der Arbeitsmaschinen rasch voran. Um die Arbeitsmaschinen und deren Werkzeuge zu automatisieren, ist es notwendig durch eine Zustandsschätzung die Lage und die Orientierung der Arbeitsmaschine und der Werkzeuge zu kennen. Die Lage und die Orientierung von mehreren zueinander beweglichen Modulen, die über Gelenke miteinander verbunden sind, werden mittels Inertialsensoren, die an den Modulen angeordnet sind, ermittelt.
In 1 ist als Beispiel für eine Arbeitsmaschine ein Bagger 1, mit einem Unterbau U und einem Oberwagen L1 dargestellt, wobei der Oberwagen L1 über ein erstes Gelenk J1 gegenüber dem Unterwagen U in der Horizontalen drehbar ist. Am Oberbau L1 ist eine erste Sensoreinheit S1 angeordnet, welche Inertialsensoren, die die Linearbeschleunigung und/oder die Drehrate des Oberbaus gegenüber einem ortsfesten Referenzkoordinatensystem messen, sowie ein Magnetometer, welches das Erdmagnetfeld an dieser Stelle misst, aufweist. Der Bagger 1 weist einen Baggerarm 2 mit weiteren Gliedern Ausleger L2, Verstellausleger L3 und Stiel L4, sowie eine Schaufel 3 bzw. L5 auf. Der Ausleger L2 ist über ein zweites Gelenk J2 mit dem Oberwagen L1 des Baggers 1 verbunden, der Verstellausleger L3 ist über ein drittes Gelenk J3 mit dem Ausleger L2 verbunden, der Stiel L4 ist über ein viertes Gelenk J4 mit dem Verstellausleger L3 verbunden und die Schaufel L5 ist über ein fünftes Gelenk J5 mit dem Stiel L4 verbunden. An jedem Glied L1, L2, L3, L4, L5 ist jeweils eine Sensoreinheit S1, S2, S3, S4, S5 angeordnet, die Inertialsensoren sowie ein Magnetometer aufweist. Die Inertialsensoren sind Beschleunigungssensoren und Drehratensensoren, welche die Linearbeschleunigung und die Drehrate des jeweiligen Sensors gegenüber einem ortsfesten Referenzkoordinatensystem messen. Zudem können die Sensoreinheiten S1, S2, S3, S4, S5 Gelenkwinkelsensoren aufweisen, die jeweils den Gelenkwinkel jedes Gelenks J1, J2, J3, J4, J5 messen. Mit W ist ein globales Koordinatensystem gekennzeichnet, in dem sich der Bagger 1 befindet.
Ein an sich bekanntes Verfahren zur Zustandsschätzung ist für den Oberwagen L1 und Ausleger L2 in der 2 dargestellt und wird im Folgenden anhand dieser Figur kurz erläutert:

Für den Unterwagen U werden eine Transformationsmatrix Twu und ein daraus ableitbares Einheitsquaternion q_wu, welche die Lage und die Orientierung des Unterwagens U im globalen Koordinatensystem W angeben, bereitgestellt 10. Gleichzeitig oder nacheinander nehmen die Sensoreinheiten S1, S2 (in 2 nicht gezeigt auch die weiteren Sensoren S3, S4, S5), genauer die Inertialsensoren und die Magnetometer, Messsignale auf 20, 30. Als Messsignale wird die mit ω bezeichnete Winkelgeschwindigkeit, die mit a bezeichnete Beschleunigung sowie das mit m bezeichnete Magnetfeld aufgenommen. Der linksseitige Index gibt in 2 an, in welchem Bezugskoordinatensystem der jeweilige Messwert aufgenommen wurde. Die gemessene Winkelgeschwindigkeit _s1ω_mess, die gemessene Beschleunigung _s1a_mess und das gemessene Magnetfeld _s1m_mess für den Oberwagen L1 durchlaufen einen Filter 21, wodurch unter anderem ein Quaternion q_W,L1, das die Rotation des Oberwagens L1 gegenüber dem globalen Koordinatensystem W repräsentiert, und die geschätzte Winkelgeschwindigkeit _L1ω_est des Oberwagens L1 ermittelt werden. Für das erste Gelenk J1 wird nun mittels des Quaternion q_W,L1 des Oberwagens L1 und des Einheitsquaternions des Unterwagens U ein erster Gelenkwinkel Θ₁ ermittelt 40 und daraus wird dann im Anschluss die Transformationsmatrix T_U,L1 für den Übergang zwischen dem Unterwagen U und dem Oberwagen L1 ermittelt 41. Schließlich erfolgt eine Matrixmultiplikation 42 der Transformationsmatrix Tw,u für den Unterwagen U mit der Transformationsmatrix T_U,L1 für den Übergang zwischen dem Unterwagen U und dem Oberwagen L1, um die Transformationsmatrix T_W,L1 für den Übergang zwischen dem globalen Koordinatensystem W und dem Oberwagen L1 zu erhalten.

In analoger Weise wird die gemessene Winkelgeschwindigkeit _S2ω_mess, die gemessene Beschleunigung _S2a_mess und das gemessene Magnetfeld _S2m_mess für den Ausleger L2 des Baggerarms 2 vom zweiten Sensor S2 aufgenommen 30 und durchläuft einen Filter 31, wodurch unter anderem ein Quaternion q_W,L2, das die Rotation des Auslegers L2 gegenüber dem globalen Koordinatensystem W repräsentiert, und die geschätzte Winkelgeschwindigkeit _L2ω_est des Auslegers L2 ermittelt werden. Für das zweite Gelenk J2 wird nun mittels des Quaternions q_W,L2 des Auslegers L2 und mittels des Quaternions q_W,L1 des Oberwagens L1 ein zweiter Gelenkwinkel Θ₂ ermittelt 50. Daraus wird im Anschluss unter Verwendung der als bekannt angenommen kinematischen Parameter, wie z.B. der Denavit-Hartenberg Parameter, des artikulierten Armes in Analogie zur sogenannten Vorwärtskinematik von Roboterarmen die Transformationsmatrix T_L1,L2 für den Übergang zwischen dem Oberwagen L1 und dem Ausleger L2 ermittelt 51. Schließlich erfolgt eine Matrixmultiplikation 52 der Transformationsmatrix T_W,L1 für den Übergang zwischen dem globalen Koordinatensystem W und dem Oberwagen L1 und der Transformationsmatrix T_L1,L2 für den Übergang zwischen dem Oberwagen L1 und dem Ausleger L2, um die Transformationsmatrix T_W,L2 für den Übergang zwischen dem globalen Koordinatensystem W und dem Ausleger L2 zu erhalten.
Für die weiteren Glieder kann das Verfahren in analoger Weise weitergeführt werden.
Eine detaillierte Beschreibung des Verfahrens zur Berechnung der Vorwärtskinematik aus Gelenkwinkeln für stationäre Maschinen ist z. B. in der Abhandlung von Spong, Mark W., Seth Hutchinson, und Mathukumalli Vidyasagar, „Robot modeling and control“, Vol. 3. New York: Wiley, 2006 gegeben, auf die insoweit verwiesen wird.
Offenbarung der Erfindung
Bei Arbeitsmaschinen, die typischerweise eine großen Masse Metall aufweisen, werden die Messungen der Magnetometer durch die große Masse Metall und die dadurch entstehenden Änderungen des Magnetfelds so verändert, dass sie häufig unbrauchbar oder zumindest unzuverlässig sind. Es ist auch möglich, dass die Messungen der einzelnen Magnetometer unterschiedlich beeinflusst werden, sodass Zustandsschätzungen für die jeweiligen Module bzw. Glieder auseinanderdriften und dadurch bei den Zustandsschätzungen Konfigurationen der Orientierung und/oder der Lage der Module bzw. Glieder entstehen, die gemäß der Kinematik nicht möglich sind. Als Beispiel gibt eine Zustandsschätzung für zwei Module, die über ein Gelenk verbunden sind, jeweils unterschiedliche Gierwinkel an, obwohl das durch die Kinematik ausgeschlossen sein sollte. Im Rahmen der vorliegenden Erfindung kann unter einem Modul ein Glied verstanden werden.
Es wird vorgeschlagen, zumindest ein Paar aus Referenzvektor und „gemessenem“ Vektor zu ermitteln, welches kinematische Zusammenhänge zumindest eines der Gelenke und der beiden mit dem Gelenk verbundenen Module repräsentiert. Da hierbei höchstens indirekt Messungen durchgeführt werden, sondern das Ermitteln der genannten Vektoren vielmehr als sogenannte virtuelle Messungen betrachtet werden kann, wird der zumindest eine Vektor auch als virtueller Vektor bezeichnet werden. Dieses Vektorpaar dann in die Zustandsschätzung der Module ein. Die kinematischen Zusammenhänge können insbesondere kinematische Bedingungen (engl.: kinematic constraints) sein, welche die Begrenzungen der Komponenten repräsentieren. So kann sich ein Gelenk typischerweise nur bis zu einem maximalen Winkel drehen und feste Module können sich nicht überlappen.
Besonders vorteilhaft werden Vektorpaare ermittelt, welche die kinematischen Zusammenhänge repräsentieren. Ein erstes Vektorpaar repräsentiert dabei den kinematischen Zusammenhang, dass das Gelenk aus Sicht jeder der beiden Module, die mit dem Gelenk verbunden sind, die gleiche Gelenkachse aufweist. Der Ausdruck „aus Sicht des ...“ gibt an, welches Koordinatensystem zur Betrachtung herangezogen wird. Mit anderen Worten, ist die Lage und Orientierung des Gelenks bzw. der Gelenkachse unabhängig von welchem Modul aus sie ermittelt wird und ist somit für beide Module gleich. Demzufolge wird durch den ersten Vektor auch der kinematische Zusammenhang repräsentiert, dass das Gelenk aus Sicht jeder der beiden Module die gleiche Gelenkachse aufweist.
Ein zweites Vektorpaar repräsentiert den kinematischen Zusammenhang, dass ein gemessener Gelenkwinkel aus Sicht eines Moduls, das mit dem Gelenk verbundenen ist, zumindest eine Achse des anderen Moduls, das mit dem Gelenk verbunden ist, vorgibt. Mit anderen Worten, wurde der Gelenkwinkel gemessen und ist die Orientierung des einen Moduls bekannt, so ist auch die Orientierung einer Achse des anderen Moduls bekannt. Der Gelenkwinkel kann z. B. durch einen Gelenkwinkelsensor gemessen werden.
Bevorzugt fließt das zumindest eine Vektorpaar bei einer Fusionierung der Sensordaten des Inertialsensors, der dem jeweiligen Modul zugeordnet ist, ein. Vorzugsweise werden die beiden obengenannten Vektorpaare für jeden Inertialsensor ermittelt und fließen bei der zugehörigen Filterung der Sensordaten dieses Sensors ein. Besonders bevorzugt erfolgt die Fusionierung durch eine Filterung. Es können aber auch andere Sensorfusionsverfahren verwendet werden, z. B. solche, die auf einer Auswertung von Graphen basieren.
Die Module sind typischerweise entlang einer kinematischen Kette angeordnet, das heißt die Bewegung eines Moduls hängt von der Bewegung des zuvor angeordneten Moduls ab. Vorzugsweise wird die Ermittlung der Vektoren für ein Modul nach dem anderen in Reihenfolge, beginnend bei einem ersten Modul, das mit einer festen Referenz, insbesondere im globalen Koordinatensystem, verbunden ist, durchgeführt.
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 kinematischen Zusammenhänge bei der Zustandsschätzung miteinfließen zu lassen.
Das Verfahren findet Anwendung bei einer Arbeitsmaschine, die einen mehrgliedrigen, artikulierten Arm aufweist. Ein Beispiel für solch eine Arbeitsmaschine ist ein Bagger mit einem Schaufelarm. Die Module entsprechen dabei den Gliedern des Arms, können zusätzlich auch weiteren Teilen des Baggers, wie z. B. einem Oberbau entsprechen.
Figurenliste
Ein Ausführungsbeispiel der Erfindung ist in den Zeichnungen dargestellt und in der nachfolgenden Beschreibung näher erläutert.

1 zeigt eine Arbeitsmaschine in Form eines Baggers gemäß dem Stand der Technik, an dem das erfindungsgemäße Verfahren durchgeführt werden kann.
2 zeigt ein Ablaufdiagramm des Verfahrens zur Zustandsschätzung gemäß dem Stand der Technik.
3 zeigt ein Ablaufdiagramm des Verfahrens zur Zustandsschätzung gemäß einer Ausführungsform der Erfindung.

Ausführungsbeispiel der Erfindung
Im Folgenden wird eine Ausführungsform des erfindungsgemäßen Verfahrens zur Zustandsschätzung von Lage und Orientierung von beweglichen Modulen eines Baggers 1 aus 1 beschrieben. Als bewegliche Module werden ein zu einem Unterwagen U drehbarer Oberwagen L1, Ausleger L2, Verstellausleger L3, Stiel L4 und Schaufel L5 angesehen. Die beweglichen Module sind entlang einer kinematischen Kette über Gelenke J1, J2, J3, J4, J5 miteinander verbunden und weisen jeweils Sensoreinheiten S1, S2, S3, S4, S5, die Inertialsensoren und Magnetometer umfassen, auf. Eine detaillierte Beschreibung wurde oben im Abschnitt 'Stand der Technik' gegeben. 3 zeigt ein Ablaufdiagramm der Ausführungsform des erfindungsgemäßen Verfahrens. Gleiche Schritte wie im in 2 gezeigten Verfahren gemäß dem Stand der Technik sind mit gleichen Bezugszeichen gekennzeichnet und auf deren erneute Beschreibung wird verzichtet.
Bei der Notation gilt zu beachten, dass der rechtseitige Index angibt, zwischen welchen Modulen bzw. Koordinatensystemen die Bewegung erfolgt und der linksseitige Index angibt aus welchem Koordinatensystem die Bewegung betrachtet wird (der Ausdruck „aus Sicht des ...“ gibt an, welches Koordinatensystem zur Betrachtung herangezogen wird).
Für jeden Sensor S1, S2, S3, S4, S5 erfolgen jeweils eine Ermittlung 100, 110 des Vektorpaars _Sin_mess,i, _Wn_ref,i und eine Ermittlung 101, 111 des zweiten Vektorspaars _Sio_mess,i, _Wo_ref,i (Index i steht hier für ein beliebiges Modul mit zugehörigem Sensor), die nachfolgend detailliert erläutert werden. Diese beiden Vektorpaare o und n ersetzen oder ergänzen den im Stand der Technik gemessenen Magnetfeldvektor _Sm_mess (siehe 2). Gemäß der Formel 1 fließen die Vektoren _Sin_mess,i, _Wn_ref,i und _Sio_mess,i, _Wo_ref,i bei der Filterung 21, 31 ein, um die Abweichung bzw. den Fehler zwischen den virtuellen Messungen und den erwarteten Referenzgrößen_Wn_ref,i und _Wo_ref,i zu minimieren: $\begin{matrix} S i^{n} m e s s i, i - {(R_{W, S i})}^{T} \cdot w^{n} r e f, i \\ S i^{O} m e s s, i - {(R_{W, S i})}^{T} \cdot w^{O} r e f, i \end{matrix}$
_Sin_mess,i und _Sio_mess,i sind die im Sensorkoordinatensystem gemessenen (bzw. virtuell gemessenen) Vektoren, R_W,Si gibt die Orientierung des Sensors Si relativ zum globalen Koordinatensystem W an und _Wn_ref,i und _Wo_ref,i sind die Vektoren als Referenz aus Sicht des globalen Koordinatensystems W.
Im Folgenden wird die Ermittlung 100, 101 der Vektorpaare für die erste Sensoreinheit S1 beschrieben. Die erste Sensoreinheit S1 ist am Oberwagen L1 angeordnet und weist Inertialsensoren sowie einen Gelenkwinkelsensor auf. Der Oberwagen L1 ist entlang einer kinematischen Kette über ein erstes Gelenk J1 mit dem Unterwagen U verbunden. Es wird bei der Ermittlung 100 des ersten Vektorpaars _S1n_mess,1, _Wn_ref,1 wie folgt vorgegangen:

Die Gelenkachse des ersten Gelenks J1, welches die aufeinanderfolgenden Module Oberwagen L1 und Unterwagen U verbindet, kann relativ zu beiden Koordinatensystemen der Module direkt angegeben werden. Wird der Vektor _S1n_mess,1 des ersten Vektorpaars für den auf dem Oberwagen L1 angeordneten ersten Sensor S1 unter Verwendung der als bekannt angenommenen Orientierung zwischen Sensor 1 und Oberwagen L1 einerseits und der Vektor _Wn_ref,i unter Verwendung der Orientierungsschätzung des Vorgängerglieds L1 andererseits angegeben, so sollten die beiden so ermittelten Vektoren, sobald sie in ein gemeinsames Koordinatensystem transformiert werden, aufgrund des kinematischen Zusammenhangs identisch oder parallel sein.

Entsprechend der Denavit-Hartenberg-Konvention aus der Abhandlung von Spong et. al. „Robot modeling and control“ (siehe oben) entspricht die Gelenkachse der z-Achse des vorherigen Moduls. Als vorheriges Modul ist hier das Modul anzusehen, das entlang einer kinematischen Kette, die vom unbewegten Modul ausgeht, vor dem aktuellen Modul angeordnet und mit diesem direkt verbunden ist. In diesem Fall ist das vorherige Modul also der Unterwagen U: $_{U} n = e_{z}$
Es kann gemäß Formel 3 eine virtuelle Messung des ersten Vektors im Koordinatensystem des ersten Sensors S1, der an dem Oberwagen L1 angeordnet ist, durchgeführt werden. $_{S 1^{n} m e s s,1} = {(R_{L 1, S 1})}^{T} {(R_{U, L 1})}^{T} e_{z}$
R_L1,S1 bezeichnet einen konstanten Applikationsparameter, der die Orientierung des ersten Sensors S1 relativ zum Oberwagen L1 repräsentiert und als bekannt angenommen werden kann. R_U,L1 repräsentiert den Rotationsanteil der Transformationsmatrix T_U,L1 zwischen dem Oberwagen L1 und dem Unterwagen U (siehe 2), also der sogenannten A-Matrix der Denavit-Hartenberg Konvention. In der eingangs genannten Abhandlung von Spong et. al. „Robot modeling and control“ (Gleichung 3.10) ist diese folgendermaßen definiert: $A_{i} = R o t_{z, θ_{i}} T r a n s_{z, d_{i}} T r a n s_{x, a_{i}} R o t_{x, a_{i}}$
θ_i bezeichnet hierbei den variablen Gelenkwinkel und d_i, α_i und a_i sind konstante kinematische Gelenkparameter. Für den Rotationsteil gilt daher: $\begin{array}{l} R_{U, L 1} = R o t_{z, θ_{i}} R o t_{x, a_{i}} \\ R_{L 1, U} = R_{U, L 1}^{T} = R o t_{x, - a_{i}} R o t_{z -, θ_{i}} \end{array}$
R_L1,U bezeichnet die inverse bzw. transponierte Rotationsmatrix von R_U,L1.
Da ein parallel zur z-Achse stehender Vektor invariant gegenüber Rotation um die z-Achse ist, lässt sich Formel 3 weiter vereinfachen, so dass sie nur noch von konstanten Parametern abhängt, die entweder die Orientierung R_L1,S1 des ersten Sensors S₁ in Bezug auf den Oberwagen L₁ oder die Kinematik des Gelenks, ausgedrückt durch den Parameter α_i, betreffen: $_{S 1^{n} m e s s,1} = {(R_{L 1, S 1})}^{T} R o t_{x, - a_{i}} e_{z}$
Gleichzeitig kann der erste Vektor _Wn_ref als Referenz aus Sicht des globalen Koordinatensystems W gemäß der Formel 7 über die Zustandsschätzung der Orientierung des vorherigen Glieds, also des Unterwagens U, angegeben werden: $_{W^{n}_{r e f,1}} = R_{W, U} e_{z}$
Es gilt zu beachten, dass weder für die Beschreibung des ersten Vektors _S1n_mess,1 der virtuellen Messung für den ersten Sensor S1 noch für die Beschreibung des ersten Vektors _Wn_ref,1 als Referenz aus Sicht des globalen Koordinatensystems W die Zustandsschätzung für das betrachtete Glied, also den Oberwagen L1, oder für den ersten Sensor S1 verwendet wurde. Der in Formel 8 ausgedrückte Zusammenhang (entspricht Formel 1 oben für den ersten Sensor S1) kann daher bei der Filterung 21 zur Zustandsschätzung der Orientierung R_W,S1 des ersten Sensors S1 einfließen, um den Differenzvektor zwischen der virtuellen Messung und der erwarteten Referenz _Wn_ref,1 zu minimieren: $_{S 1^{n} m e s s,1} = {(R_{W, S 1})}^{T} \cdot_{W^{n} r e f,1}$
Es wird bei der Ermittlung 101 des zweiten Vektorspaars _s1o_mess1, _Wo_ref,1 (weiterhin für den ersten Sensor S1) wie folgt vorgegangen:
Es kann gemäß Formel 9 eine virtuelle Messung des zweiten Vektors im Koordinatensystem des ersten Sensors S1, der an dem Oberwagen L1 angeordnet ist, durchgeführt werden. $_{S 1^{O} m e s s,1} = (R_{S 1, L 1}) e_{x}$
R_S1,L1 bezeichnet die inverse bzw. transponierte Rotationsmatrix von R_L1,S1 und ist somit ebenfalls ein konstanter Applikationsparameter, der die Orientierung des ersten Sensors S1 relativ zum Oberwagen L1 repräsentiert und als bekannt angenommen werden kann.
Gleichzeitig kann der zweite Vektor _Wo_ref,1 als Referenz aus Sicht des globalen Koordinatensystems W gemäß der Formel 10 über die Zustandsschätzung der Orientierung des vorherigen Glieds, also des Unterwagens U, und dem Rotationsanteil R_U,L1 der Transformationsmatrix T_U,L1 zwischen dem Oberwagen L1 und dem Unterwagen U angegeben werden: $\begin{array}{l} _{W^{O} r e f,1} = R_{W, U} R_{U, L 1} e_{x} = R_{W, U} R o t_{z, θ_{1}} R o t_{x, α_{1}} e_{x} \\ _{W^{O} r e f,1} = R_{W, U} R o t_{z, θ_{1}} e_{x} \end{array}$
Dieser Vektor hängt lediglich von der Orientierung des Unterwagens U in Bezug auf das Referenzkoordinatensystem W und dem gemessenen Gelenkwinkel θ₁ ab. Allgemeiner ausgedrückt hängt dieser Vektor nur von der Orientierungsabschätzung des Vorgängermoduls und dem Gelenkwinkel zwischen den beiden Modulen ab.
Es gilt auch hier zu beachten, dass weder für die Beschreibung des zweiten Vektors _S1o_mess,1 für den ersten Sensor S1 noch für die Beschreibung des zweiten Vektors _Wo_ref,1 als Referenz aus Sicht des globalen Koordinatensystems W die Zustandsschätzung für das betrachtete Modul, also den Oberwagen L1, oder für den ersten Sensor S1 verwendet wurde. Der in Formel 11 ausgedrückte Zusammenhang (entspricht Formel 11 unten für den ersten Sensor S1) kann daher bei der Filterung 21 für den ersten Sensor einfließen, um den Differenzvektor zwischen der virtuellen Messung und der erwarteten Referenz _Wo_ref,1 zu minimieren: $_{S 1^{O} m e s s,1} = {(R_{W, S 1})}^{T} \cdot_{W^{O} r e f,1}$
Die in 3 dargestellte Ermittlung 110 des ersten Vektorpaars _S2n_mess,2, _Wn_ref,2 und Ermittlung 111 des zweiten Vektorpaars _S2o_mess,2, _Wo_ref,2 für den zweiten Sensor S2 kann in analoger Weise durchgeführt werden. Dies gilt auch für die Ermittlung der Vektoren für die anderen Sensoren S3, S4, S5, die der Übersicht halber in 3 nicht gezeigt sind.

Claims

Verfahren zur Zustandsschätzung von Lage und Orientierung von mehreren zueinander über Gelenke (J1, J2, J3, J4, J5) beweglichen Modulen (L1, L2, L3, L4, L5) eines gemeinsamen Systems mittels Inertialsensoren (S1, S2, S3, S4, S5), die an den Modulen(L1, L2, L3, L4, L5) angeordnet sind, dadurch gekennzeichnet, dass zumindest ein Vektorpaar (_Sin_mess,i, _Wn_ref,i oder _Sio_mess,i, _Wo_ref,i) ermittelt (100, 101, 110, 111) wird, welches kinematische Zusammenhänge zumindest eines der Gelenke (J1, J2, J3, J4, J5) und der beiden mit dem Gelenk (J1, J2, J3, J4, J5) verbundenen Modulen (L1, L2, L3, L4, L5) repräsentiert, und zumindest ein Vektorpaar (_Sin_mess,i, _Wn_ref,i oder _Sio_mess,i, _Wo_ref,i) in die Zustandsschätzung einfließt.
Verfahren nach Anspruch 1, gekennzeichnet durch ein erstes Vektorpaar (_Sin_mess,i, _Wn_ref,i), des den kinematischen Zusammenhang repräsentiert, dass das Gelenk (J1, J2, J3, J4, J5) aus Sicht jeder der beiden mit dem Gelenk verbundenen Module (L1, L2, L3, L4, L5) die gleiche Gelenkachse aufweist.
Verfahren nach Anspruch 1 oder 2, gekennzeichnet durch ein zweites Vektorpaar (_sio_mess,i, _Wo_ref,i), das den kinematischen Zusammenhang repräsentiert, dass ein gemessener Gelenkwinkel (Θ) aus Sicht eines der mit dem Gelenk (J1, J2, J3, J4, J5) verbundenen Glieder (L1, L2, L3, L4, L5) zumindest eine Achse (e_x) des anderen mit dem Gelenk (J1, J2, J3, J4, J5) verbundenen Glieds (L1, L2, L3, L4, L5) vorgibt.
Verfahren nach einem der vorhergehenden Ansprüche, dadurch gekennzeichnet, dass das zumindest eine Vektorpaar (_Sin_mess,i, _Wn_ref,i oder _Sio_mess,i, _Wo_ref,i) bei einer Fusionierung der Sensordaten des dem Modul (L1, L2, L3, L4, L5) zugeordneten Inertialsensors (S1, S2, S3, S4, S5) einfließt.
Verfahren nach Anspruch 4, dadurch gekennzeichnet, dass die Fusionierung der Sensordaten durch eine Filterung (21, 31) erfolgt.
Computerprogramm, welches eingerichtet ist, jeden Schritt des Verfahrens nach einem der Ansprüche 1 bis 5 durchzuführen.
Maschinenlesbares Speichermedium, auf welchem ein Computerprogramm nach Anspruch 6 gespeichert ist.
Elektronisches Steuergerät, welches eingerichtet ist, um mittels eines Verfahrens nach einem der Ansprüche 1 bis 5 eine Abschätzung von Lage und Orientierung von mehreren zueinander beweglichen Modulen durchzuführen.
Verwendung des Verfahrens gemäß einem der Ansprüche 1 bis 5 bei einer Arbeitsmaschine (1), die einen mehrgliedrigen, artikulierten Arm (2) aufweist, wobei die Module (L2, L3, L4, L5) den Gliedern des Arms (2) entsprechen.