DE102019201099A1 - Method for estimating the position and orientation of several moving modules in a common system - Google Patents

Abstract

The invention relates to a method for estimating the position and orientation of a plurality of mutually movable modules of a common system by means of inertial sensors (S1, S2) which are arranged on the modules. A linear acceleration (α, a,) that occurs during a dynamic movement is compensated for by means of sensor data from the inertial sensors (S1, S2) before the condition is estimated.

Description

The present invention relates to a method for estimating the state of the position and orientation of a plurality of modules of a common system that can be moved by means of joints, in which a linear acceleration of sensors of the common system is compensated for before the condition is estimated. The invention further relates to a computer program that executes every step of the method when it runs on a computing device, and to a machine-readable storage medium that stores the computer program. Finally, the invention relates to an electronic control device which is set up to carry out the method according to the invention.

State of the art

Nowadays, automation in the field of work machines is progressing rapidly. In order to automate the work machines and their tools, it is necessary to know the position and orientation of the work machine and the tools by means of an assessment of the condition. The position and the orientation of several mutually movable modules, which are connected to one another in particular via joints, are determined by means of inertial sensors which are arranged on the modules.

In 1 is an excavator as an example of a work machine 1 , with an undercarriage U and an uppercarriage L1 shown, the superstructure L1 about a first joint J1 opposite the undercarriage U is rotatable in the horizontal. On the superstructure L1 is a first sensor unit S1 arranged, which inertial sensors, which measure the acceleration and / or the rate of rotation of the superstructure compared to a fixed reference coordinate system, and a magnetometer, which measures the earth's magnetic field at this point. The excavator 1 has an excavator arm 2nd with further links boom L2 , Adjustable boom L3 , Stem L4 and a shovel L5 on. The uppercarriage L1 is about a second joint J2 with the boom L2 of the excavator 1 connected, The boom L2 is about a third joint J3 with the adjustable boom L3 connected, adjustable boom L3 is about a fourth joint J4 with the stem L4 connected and the stem L4 is about a fifth joint J5 with the shovel L5 connected. On every link L1 , L2 , L3 , L4 , L5 is a sensor unit S1 , S2 , S3 , S4 , S5 arranged, which has inertial sensors and a magnetometer. The inertial sensors are acceleration sensors and rotation rate sensors, which measure the linear acceleration and the rotation rate of the respective sensor compared to a fixed reference coordinate system. In addition, the sensor units S1 , S2 , S3 , S4 , S5 Joint angle sensors each have the joint angle of each joint J1 , J2 , J3 , J4 , J5 measure up. With W is a global coordinate system characterized in which the excavator 1 located.

• Für den Unterbau U werden eine Transformationsmatrix Twu und ein daraus ableitbares Einheitsquaternion Qwu, welche die Lage und die Orientierung des Unterbaus 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 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_WL1 des Oberbaus L1 und des Einheitsquaternions des Unterbaus 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 Twu 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.

A known method for estimating the condition is for the superstructure L1 and adjustable boom L2 in the 2nd shown and is briefly explained below with reference to this figure:

• For the substructure U become a transformation matrix Twu and a unit quaternion Qwu that can be derived from it, which shows the location and orientation of the substructure U in the global coordinate system W specify, provided 10. Simultaneously or in succession take the sensor units S1 , S2 (in 2nd the other sensors are also not shown S3 , S4 , S5 ), more precisely the inertial sensors and the magnetometers, measurement signals 20, 30th . The angular velocity designated ω, the acceleration designated a and the magnetic field designated m are recorded as measurement signals. The left-hand index gives in 2nd in which reference coordinate system the respective measured value was recorded. The measured angular velocity _S1 ω _mess , the measured acceleration _S1 a _mess and the measured magnetic field _S1 m _mess for the superstructure pass through a filter 21 , which causes, among other things, a quaternion q _{W, L1} that the rotation of the superstructure L1 versus the global coordinate system W represents, and the estimated angular velocity _L1 ω _{est of} the superstructure L1 be determined. For the first joint J1 is now using the quaternion q _{WL1 of} the superstructure L1 and the uniform quaternion of the substructure U a first joint angle Θ _{1 is} determined 40 and this then becomes the transformation matrix T _{U, L1} for the transition between the undercarriage U and the uppercarriage L1 determined 41 . Finally, the matrix is multiplied 42 the Twu transformation matrix for the undercarriage U with the transformation matrix T _{U, L1} for the transition between the undercarriage U and the uppercarriage L1 to the transformation matrix T _{W, L1} for the transition between the global coordinate system W and the uppercarriage L1 to obtain.

The measured angular velocity _S2 ω _mess , the measured acceleration _S2 a _mess and the measured magnetic field _S2 m _{mess are} measured in an analogous manner for the boom L2 of the excavator arm 2nd from the second sensor S2 added 30th and goes through a filter 31 , which, among other things, creates a quaternion q _WL2 , which _controls the rotation of the boom L2 versus the global coordinate system W represents, and the estimated Angular velocity _L2 ω _{est of} the boom L2 be determined. For the second joint J2 is now by means of the quaternion q _{W, L2 of} the boom L2 and by means of the quaternion q _{W, L1 of} the superstructure L1 a second joint angle Θ _{2 is} determined 50 . From this, the transformation matrix T _{L1, L2} for the transition between the superstructure is then made using the kinematic parameters assumed to be known, for example the Denavit-Hartenberg parameters, of the articulated arm in analogy to the so-called forward kinematics of robot arms L1 and the boom L2 determined 51 . Finally, the matrix is multiplied 52 the transformation matrix T _{W, L1} for the transition between the global coordinate system W and the uppercarriage L1 and the transformation matrix T _{L1, L2} for the transition between the superstructure L1 and boom L2 to the transformation matrix T _{W, L2} for the transition between the global coordinate system W and the boom L2 to obtain.

The procedure can be continued in an analogous manner for the other links.

A detailed description of the method for calculating the forward kinematics from joint angles for stationary machines is e.g. B. in the treatise by Spong, Mark W ., Seth Hutchinson, and Mathukumalli Vidyasagar, "Robot modeling and control", Vol. 3. New York: Wiley, 2006, to which reference is made in this regard.

Disclosure of the invention

A work machine typically performs a dynamic movement. The machine is accelerated during this dynamic movement. Accordingly, linear acceleration occurs for sensors of the working machine during the measurement, which above all changes the measurement signals from inertial sensors, above all from acceleration sensors. As a result, a state estimate of the position and orientation of several movable modules of a common system, i.e. H. the working machine and its tools, distorted during dynamic movement.

It is now proposed to compensate the linear acceleration that occurs during the dynamic movement by means of sensor data from the inertial sensors before the state estimation. As mentioned at the beginning, the inertial sensors are assigned to the modules, the compensation preferably being carried out individually for each sensor.

To determine the global coordinate system, the direction of earth's gravity is usually determined by one of the inertial sensors. However, if the described linear acceleration occurs during the dynamic movement of the working machine, the direction is determined from a combination of gravity and a portion of the linear acceleration and the result is falsified. It is therefore preferably provided to compensate for the linear acceleration when determining the direction of gravity.

As already described, the inertial sensors are assigned to the respective modules. The sensor data measured by the inertial sensors and used to compensate for the linear acceleration preferably include an acceleration of the assigned module. In addition, the measured sensor data can include an angular velocity of the assigned module. The acceleration and the angular velocity of each module are preferably recorded and used to compensate for the linear acceleration for this sensor.

As already described, preferably the acceleration, the sensor data of a sensor are advantageously used for the recursive calculation of a further sensor. The further sensor is preferably assigned to a module which is connected directly to the module of the first-mentioned sensor. In other words, the linear acceleration of a sensor on a module is compensated for with reference to the sensor data, in particular the acceleration and the angular velocity, for the sensor of the module arranged directly in front of it.

The modules are typically arranged along a kinematic chain, i.e. the movement of a module depends on the movement of the previously arranged module. In this case, the recursive calculation of the linear acceleration preferably takes place along a kinematic chain of the modules. In other words, the calculation of the linear acceleration of the modules is carried out successively, one after the other, starting with a first module that is connected to a fixed reference, in particular in the global coordinate system.

The computer program is set up to carry out every step of the method, in particular if it is carried out on a computing device or control device. It enables the method to be implemented in a conventional electronic control unit without having to make structural changes to it. For this purpose, it is stored on the machine-readable storage medium.

By loading the computer program onto a conventional electronic control device, the electronic control device is obtained which is set up to carry out the compensation of the linear acceleration.

The method is used in a work machine that has a multi-part, articulated arm. An example of such a work machine is an excavator with a bucket arm. The modules correspond to the limbs of the arm, can also also other parts of the excavator, such as. B. correspond to a superstructure.

Figure list

• 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.

An embodiment of the invention is shown in the drawings and explained in more detail in the following description.

• 1 shows a work machine in the form of an excavator according to the prior art, on which the inventive method can be carried out.
• 2nd shows a flowchart of the method for state estimation according to the prior art.
• 3rd shows a flow diagram of the method for state estimation according to an embodiment of the invention.

Embodiment of the invention

The following is an embodiment of the method according to the invention for estimating the state of the position and orientation of movable modules of an excavator 1 out 1 described. As moving modules, they become an undercarriage U rotating superstructure L1 as well as outriggers L2 , Adjustable boom L3 , Stem L4 and a shovel L5 viewed. The moving modules are along a kinematic chain over joints J1 , J2 , J3 , J4 , J5 connected to each other and each have sensor units S1 , S2 , S3 , S4 , S5 that include inertial sensors and magnetometers. A detailed description has been given above in the prior art section.

3rd shows a flow chart of the embodiment of the method according to the invention. Same steps as in the 2nd Methods shown according to the prior art are identified by the same reference numerals and their description is omitted.

With the notation it should be noted that the right-hand index indicates between which modules or coordinate systems the movement takes place and the left-hand index specifies from which coordinate system the movement is viewed (the expression "from the perspective of ..." indicates which coordinate system is used for consideration).

For every sensor S1 , S2 , S3 , S4 , S5 linear acceleration is compensated. There is a calculation 100 , 110 the linear acceleration of the current sensor S1 , S2 , S3 , S4 , S5 from the determined quantities for the linear acceleration of the previous sensor, the relative homogeneous transformation matrix between the previous module and the current module on which the current sensor is arranged, the angular velocity of the previous sensor relative to the global coordinate system and the angular velocity of the current sensor relative to the global coordinate system. The previous module is the module that is arranged in front of the current module along a kinematic chain that starts from the stationary module and is directly connected to it. The previous sensor is the sensor that is arranged on the previous module.

• Es wird eine rekursive Berechnung der Beschleunigung Auslegers L2 relativ zum globalen Koordinatensystem W aus Sicht des Oberbaus H entlang der kinematischen Kette durchgeführt, die In Formel 1 dargestellt ist: $$\begin{array}{l}{}_{L_1}{a}_{W,\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="DE102019201099A1\_0010.png"\; state="\{\{state\}\}">L</figure-callout>}2}{=}_{L1}{a}_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{+}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\alpha }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\times }_{L1}{\mathrm{<figure-callout\; id="1"\; label="t\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">t</figure-callout>}}_L{+}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\times }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\cdot }_{L1}{\mathrm{<figure-callout\; id="1"\; label="t\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">t</figure-callout>}}_L+2_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}\\ {\times }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\mathrm{<figure-callout\; id="1"\; label="\nu \; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">\nu </figure-callout>}}_L{+}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\mathrm{<figure-callout\; id="1"\; label="\alpha \; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">\alpha </figure-callout>}}_L\end{array}$$
  
  _L1α_W,L2 bezeichnet die Linearbeschleunigung des Auslegers L2 relativ zum globalen Koordinatensystem W aus Sicht des Oberwagens L1, _L1α_W,L1 bezeichnet die Beschleunigung des Oberwagens L1 relativ zum globalen Koordinatensystem W aus Sicht des Oberwagens L1, _L1α_W,L1 bezeichnet die Winkelbeschleunigung des Oberwagens L1 relativ zum globalen Koordinatensystem W aus Sicht des Oberwagens L1, _L1ω_W,L1 bezeichnet die Winkelgeschwindigkeit des Oberwagens L1 relativ zum globalen Koordinatensystem W aus Sicht des Oberwagens L1, _L1t_L1,L2 bezeichnet die Translationskomponente der relativen homogenen Transformationsmatrix T_L1,L2, _L1v_L1,L2 bezeichnet die Lineargeschwindigkeit des Auslegers L2 relativ zum Oberwagen L1 aus Sicht des Oberwagens L1 und _L1α_L1,L2 bezeichnet die Linearbeschleunigung des Auslegers L2 relativ zum Oberwagen L1 aus Sicht des Oberwagens L1.

The following is the compensation of the linear acceleration for the inertial sensors of the second sensor unit S2 that on the boom L2 is described. The boom L2 is along a kinematic chain via a second joint J2 with the uppercarriage L1 connected. It is in the calculation 110 proceeded as follows:

• It will do a recursive calculation of the boom boom L2 relative to the global coordinate system W From the point of view of the superstructure H carried out along the kinematic chain, which is shown in Formula 1 $$\begin{array}{l}{}_{L_1}{a}_{W,L\mathrm{2nd}}{=}_{L1}{a}_{W,L1}{+}_{L1}{\alpha }_{W,L1}{\times }_{L1}{t}_{L\mathrm{1,}L\mathrm{2nd}}{+}_{L1}{\omega }_{W,L1}{\times }_{L1}{\omega }_{W,L1}{\cdot }_{L1}{t}_{L\mathrm{1,}L\mathrm{2nd}}+{\mathrm{2nd}}_{L1}{\omega }_{W,L1}\\ {\times }_{L1}{\nu }_{L\mathrm{1,}L\mathrm{2nd}}{+}_{L1}{\alpha }_{L\mathrm{1,}L\mathrm{2nd}}\end{array}$$
  
  _L1 α _{W, L2} denotes the linear acceleration of the boom L2 relative to the global coordinate system W from the perspective of the uppercarriage L1 , _L1 α _{W, L1} denotes the acceleration of the uppercarriage L1 relative to the global coordinate system W from the perspective of the uppercarriage L1 , _L1 α _{W, L1} denotes the angular acceleration of the superstructure L1 relative to the global coordinate system W from the perspective of the uppercarriage L1 , _L1 ω _{W, L1} denotes the angular velocity of the superstructure L1 relative to the global coordinate system W from the perspective of the uppercarriage L1 , _L1 t _{L1, L2} denotes the translation component of the relative homogeneous transformation matrix T _{L1, L2} , _L1 v _{L1, L2} denotes the linear velocity of the boom L2 relative to the superstructure L1 from the perspective of the uppercarriage L1 and _L1 α _{L1, L2} denotes the linear acceleration of the boom L2 relative to the superstructure L1 from the perspective of the uppercarriage L1 .

Numerical differentiation turns the angular velocity _S2 ω _{W, S2} for the second sensor S2 that as the expected angular velocity from the filter 31 is obtained, the angular acceleration _S2 α _{W, S2} for the second sensor S2 calculated and on the other hand from the angular velocity _S1 ω _{W, S1} for the first sensor S1 that as the expected angular velocity from the filter 21 is obtained, the angular acceleration _S1 α _{W, S1} for the first sensor S1 calculated.

The sensors S1 , S2 , ... are fixed on the respective modules L1 , L2 ... arranged and have a constant position to the associated module. This enables the angular velocities and accelerations to be represented from the perspective of the modules, as shown in formulas 2: $$\begin{array}{l}{}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}={\mathrm{<figure-callout\; id="1"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L{\cdot }_{\mathrm{<figure-callout\; id="1"\; label="S"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">S</figure-callout>}1}{\omega }_{W,S1}\\ {}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\alpha }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}={\mathrm{<figure-callout\; id="1"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L{\cdot }_{\mathrm{<figure-callout\; id="1"\; label="S"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">S</figure-callout>}1}{\alpha }_{W,S1}\\ {}_{L\mathrm{2nd}}{\omega }_{W,L\mathrm{2nd}}={\mathrm{<figure-callout\; id="2"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L{\cdot }_{S\mathrm{2nd}}{\omega }_{W,S1}\\ {}_{L\mathrm{2nd}}{\alpha }_{W,L\mathrm{2nd}}={\mathrm{<figure-callout\; id="2"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L{\cdot }_{S\mathrm{2nd}}{\alpha }_{W,S\mathrm{2nd}}\end{array}$$

Here R _{L1, S1 is} the rotational part of the transformation matrix between the superstructure L1 and the first sensor attached to it S1 and R _{L2, S2} the rotation portion of the transformation matrix between the boom L2 and the second sensor S2 .

However, it must be taken into account here that the coordinate systems of the modules and the sensors rotate relative to the global coordinate system. The linear acceleration _L1 α _{W, L1 of} the superstructure L1 relative to the global coordinate system W from the perspective of the uppercarriage L1 is calculated according to formula 3: $${}_{L_1}{a}_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}={\mathrm{<figure-callout\; id="1"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L{}_1{a}_{W,S1}{-}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\alpha }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\times }_{L1}{\mathrm{<figure-callout\; id="1"\; label="t\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">t\; </figure-callout>}}_L{-}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\times }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\cdot }_{L1}{\mathrm{<figure-callout\; id="1"\; label="t\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">t\; </figure-callout>}}_L$$

The relative angular velocity _L1 ω _{L1, L2} and the relative angular acceleration _L1 α _{L1, L2} between the boom L2 and the uppercarriage L1 (from the perspective of the uppercarriage L1 ) are calculated according to formulas 4: $$\begin{array}{l}{}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}\mathrm{1,}L\mathrm{2nd}}={\mathrm{<figure-callout\; id="1"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L{\cdot }_{L\mathrm{2nd}}{\omega }_{W,L\mathrm{2nd}}{-}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{W,L1}\\ {}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}\mathrm{1,}L\mathrm{2nd}}={\mathrm{<figure-callout\; id="1"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L{\cdot }_{L\mathrm{2nd}}{\omega }_{W,L\mathrm{2nd}}{-}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\alpha }_{W,\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}\end{array}$$

Assuming that it is the joints J1 , J2 ... are pure swivel joints, the linear speed and acceleration of the boom L2 relative to the superstructure L1 (from the perspective of the uppercarriage L1 ) calculated according to formulas 5: $$\begin{array}{c}{}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\mathrm{<figure-callout\; id="1"\; label="\nu \; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">\nu \; </figure-callout>}}_L{=}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}\mathrm{1,}L\mathrm{2nd}}{\times }_{L1}{\mathrm{<figure-callout\; id="1"\; label="t\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">t\; </figure-callout>}}_L& \\ {}_L{1}_{}{\mathrm{<figure-callout\; id="1"\; label="\alpha \; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">\alpha \; </figure-callout>}}_L{=}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\mathrm{<figure-callout\; id="1"\; label="\alpha \; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">\alpha \; </figure-callout>}}_L{\times }_{L1}{\mathrm{<figure-callout\; id="1"\; label="t\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">t\; </figure-callout>}}_L{+}_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}\mathrm{1,}L\mathrm{2nd}}{\times }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\omega }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}\mathrm{1,}L\mathrm{2nd}}{\times }_{L1}{\mathrm{<figure-callout\; id="1"\; label="t\; L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">t\; </figure-callout>}}_L\end{array}$$

Used in Formula 1, the linear acceleration _L1 α _{W, L2 of} the boom L2 relative to the global coordinate system W from the perspective of the uppercarriage L1 receive. Taking into account that the coordinate systems of the boom L2 and the second sensor S2 rotate, the linear acceleration _L2 α _{W, S2 of} the second sensor S2 relative to the global coordinate system W calculated from the perspective of the first link according to Formula 6: $${}_{L\mathrm{2nd}}{\alpha }_{W,S\mathrm{2nd}}={\mathrm{<figure-callout\; id="2"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L{\cdot }_{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="DE102019201099A1\_0010.png,DE102019201099A1\_0011.png"\; state="\{\{state\}\}">L</figure-callout>}1}{\alpha }_{W,L\mathrm{2nd}}{\times }_{L\mathrm{<figure-callout\; id="2"\; label="2nd\; t\; L"\; filenames="DE102019201099A1\_0010.png"\; state="\{\{state\}\}">2nd\; </figure-callout>}}{t}_L{\mathrm{2,}S\mathrm{2nd}}_{}{+}_{L\mathrm{2nd}}{\omega }_{W,L\mathrm{2nd}}{\times }_{L\mathrm{2nd}}{\omega }_{W,L\mathrm{2nd}}{\times }_{L\mathrm{<figure-callout\; id="2"\; label="2nd\; t\; L"\; filenames="DE102019201099A1\_0010.png"\; state="\{\{state\}\}">2nd\; </figure-callout>}}{t}_L{\mathrm{2,}S\mathrm{2nd}}_{}$$

die transponierte Matrix zu R_L2,S2 ist. $${}_{S2}{a}_{W,S2}=R_{L\mathrm{2,}S2}^T{\cdot }_{L2}{a}_{W,S2}$$

Finally, the linear acceleration _S2 α _{W, S2 of} the second sensor S2 relative to the global coordinate system W from the point of view of the second sensor S2 calculated according to formula 7, wherein ${\mathrm{<figure-callout\; id="2"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L^{\mathrm{2,}S\mathrm{2nd}}$

is the transposed matrix to R _{L2, S2} . $${}_{S\mathrm{2nd}}{a}_{W,S\mathrm{2nd}}={\mathrm{<figure-callout\; id="2"\; label="R\; L"\; filenames="DE102019201099A1\_0010.png"\; state="\{\{state\}\}">R\; </figure-callout>}}_L^{\mathrm{2,}S\mathrm{2nd}}{\cdot }_{L\mathrm{2nd}}{a}_{W,S\mathrm{2nd}}$$

The value of the linear acceleration _S2 α _{W, S2 of} the second sensor S2 relative to the global coordinate system W from the point of view of the second sensor S2 will be in the next measurement 30th of the second sensor S2 subtracted from the measured acceleration _S2 a _mess 111 to the corrected measured acceleration _S2 a _corr for the second sensor S2 to get which then in the filter 31 flows in.

anstelle der Beschleunigung und der Winkelgeschwindigkeit des vorherigen Moduls bzw. Sensors verwendet. In 3rd is also the calculation 100 the linear acceleration _S1 α _{W, S1 of} the first sensor S1 relative to the global coordinate system W from the point of view of the first sensor S1 for the uppercarriage L1 shown. The calculation 100 becomes analogous to the calculation 110 executed using the appropriate sizes. Because the undercarriage U is assumed to be fixed here, is used in the calculation 100 a zero vector $\overrightarrow{O}$

used instead of the acceleration and angular velocity of the previous module or sensor.

In the case of a moving base vehicle, variables derived from the vehicle's own motion estimation can also be used here. Then the linear acceleration _S1 α _{W, S1 of} the first sensor S1 relative to the global coordinate system W from the point of view of the first sensor S1 from the measured acceleration _S1 a _{mess of} the first sensor S1 subtracted 101 to the corrected measured acceleration _S1 a _corr for the first sensor S1 to get which then in the filter 21 flows in.

Linear acceleration compensation for the other sensors S3 , S4 , S5 is in 3rd Not shown for the sake of clarity, but is done in analogy to the manner described above for the second sensor S2 .

Claims (9)

Method for estimating the position and orientation of several mutually movable modules (L1, L2, L3, L4, L5) of a common system using inertial sensors (S1, S2, S3, S4, S5) that are connected to the modules (L1, L2, L3 , L4, L5), characterized in that a linear acceleration ( _S1 α _{W, S1} , _S2 α _{W, S2} ), which occurs during a dynamic movement, by means of sensor data of the inertial sensors (S1, S2, S3, S4, S5) to compensate before the condition estimation. Procedure according to Claim 1 , characterized in that the compensation of the linear acceleration ( _S1 α _W , _S1 , _S2 α _{W, S2} ) is carried out when determining the direction of gravity. Method according to one of the preceding claims, characterized in that the sensor data comprise an acceleration and / or an angular velocity of the assigned module (L1, L2, L3, L4, L5). Method according to one of the preceding claims, characterized in that the sensor data of a sensor (S1, S2, S3, S4, S5) for recursively calculating the linear acceleration ( _S1 α _{W, S1} , _S2 α _{W, S2} ) of at least one further sensor (S1 , S2, S3, S4, S5) is used. Procedure according to Claim 4 , characterized in that the recursive calculation of the linear acceleration ( _S1 α _{W, S1} , _S2 α _{W, S2} ) takes place along a kinematic chain of the modules (L1, L2, L3, L4, L5). Computer program, which is set up each step of the method according to one of the Claims 1 to 5 perform. Machine-readable storage medium on which a computer program Claim 6 is saved. Electronic control device, which is set up to use a method according to one of the Claims 1 to 5 carry out an assessment of the position and orientation of several mutually movable modules. Use of the method according to one of the Claims 1 to 5 In a work machine (1) which has a multi-part, articulated arm (2), the modules (L2, L3, L4) corresponding at least to the members of the arm (2).

Images