Method for estimating the position and orientation of multiple mobile modules of a common system

Description

[0001] The present invention relates to a method for estimating the position and orientation of several modules of a common system that are movable relative to one another via joints, in which kinematic relationships between the movable modules and the joints are incorporated into the state estimation. Furthermore, the invention relates to a computer program that executes each step of the method when executed on a computing device, as well as to a machine-readable storage medium that stores the computer program. Finally, the invention relates to an electronic control unit configured to carry out the method according to the invention. State of the art

[0002] Today, automation in the field of work machines is advancing rapidly. To automate work machines and their tools, it is necessary to know the position and orientation of the work machine and its tools through state estimation. The position and orientation of several movable modules connected by joints are determined using inertial sensors mounted on the modules.

[0003] A position detection system for an excavator is described in US 2016 / 0160472 A1.

[0004] US 2017 / 0218595 A1 discloses an excavator having sensors for detecting the position and movement of an excavator boom.

[0005] In Figur 1 As an example of a work machine, an excavator 1 is shown, with a substructure U and a superstructure L1, wherein the superstructure L1 can be rotated horizontally relative to the undercarriage U via a first joint J1. A first sensor unit S1 is arranged on the superstructure L1, which has inertial sensors that measure the linear acceleration and / or the rotation rate of the superstructure relative to a stationary reference coordinate system, as well as a magnetometer that measures the earth's magnetic field at this point. The excavator 1 has an excavator arm 2 with further links: boom L2, adjustable boom L3 and stick L4, as well as a bucket 3 or L5. The boom L2 is connected to the upper carriage L1 of the excavator 1 via a second joint J2, the adjustable boom L3 is connected to the boom L2 via a third joint J3, the stick L4 is connected to the adjustable boom L3 via a fourth joint J4 and the bucket L5 is connected to the stick L4 via a fifth joint J5.At each link L1, L2, L3, L4, and L5, a sensor unit S1, S2, S3, S4, and S5 is arranged, each of which contains inertial sensors and a magnetometer. The inertial sensors are acceleration sensors and yaw rate sensors that measure the linear acceleration and yaw rate of the respective sensor relative to a stationary reference coordinate system. Furthermore, the sensor units S1, S2, S3, S4, and S5 can contain joint angle sensors that measure the joint angle of each joint J1, J2, J3, J4, and J5. W denotes a global coordinate system in which excavator 1 is located.

[0006] A well-known procedure for condition estimation is for the uppercarriage L1 and boom L2 in the Figur 2 and is briefly explained below using this figure: For the undercarriage U, a transformation matrix T WU and a unit quaternion q WU derivable from it, which indicate the position and orientation of the undercarriage U in the global coordinate system W, are provided 10. Simultaneously or successively, the sensor units S1, S2 (in Figur 2 not shown also the other sensors S3, S4, S5), more precisely the inertial sensors and the magnetometers, measuring signals at 20, 30. The measuring signals recorded are the angular velocity denoted by ω, the acceleration denoted by a and the magnetic field denoted by m. The left-hand index indicates in Figur 2 indicates the reference coordinate system in which 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 L1 pass through a filter 21, whereby, among other things, a quaternion q W,L1 , which represents the rotation of the superstructure L1 relative to the global coordinate system W, and the estimated angular velocity L1 ω est of the superstructure L1 are determined. For the first joint J1, a first joint angle Θ 1 is now determined using the quaternion q W,L1 of the superstructure L1 and the unit quaternion of the undercarriage U 40, and from this the transformation matrix TU,L1 for the transition between the undercarriage U and the superstructure L1 is then determined 41.Finally, a matrix multiplication 42 of the transformation matrix TW,U for the undercarriage U with the transformation matrix TU,L1 for the transition between the undercarriage U and the superstructure L1 is carried out in order to obtain the transformation matrix TW,L1 for the transition between the global coordinate system W and the superstructure L1.

[0007] In an analogous manner, the measured angular velocity S2 ω mess , the measured acceleration S2 a mess and the measured magnetic field S2 m mess for the boom L2 of the excavator arm 2 are recorded 30 by the second sensor S2 and passed through a filter 31, whereby, among other things, a quaternion q W,L2 , which represents the rotation of the boom L2 with respect to the global coordinate system W, and the estimated angular velocity L2 ω est of the boom L2 are determined. For the second joint J2, a second joint angle Θ 2 is now determined using the quaternion q W,L2 of the boom L2 and the quaternion q W,L1 of the superstructure L1 50. From this, the transformation matrix T L1,L2 for the transition between the superstructure L1 and the boom L2 is then determined using the kinematic parameters of the articulated arm, which are assumed to be known, such as the Denavit-Hartenberg parameters, in analogy to the so-called forward kinematics of robot arms 51.Finally, a matrix multiplication 52 of the transformation matrix TW,L1 for the transition between the global coordinate system W and the superstructure L1 and the transformation matrix T L1,L2 for the transition between the superstructure L1 and the boom L2 is carried out in order to obtain the transformation matrix TW,L2 for the transition between the global coordinate system W and the boom L2.

[0008] The procedure can be continued in an analogous manner for the other elements.

[0009] A detailed description of the procedure for calculating forward kinematics from joint angles for stationary machines is given, for example, in the paper 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

[0010] In work machines, which typically have a large mass of metal, the measurements of the magnetometers are altered by the large mass of metal and the resulting changes in the magnetic field in such a way that they are often unusable or at least unreliable. It is also possible that the measurements of the individual magnetometers are influenced differently, so that state estimates for the respective modules or links drift apart and, as a result, configurations of the orientation and / or position of the modules or links arise in the state estimates that are not possible according to the kinematics. As an example, a state estimate for two modules connected by a joint each indicates different yaw angles, even though this should be excluded by the kinematics. In the context of the present invention, a module can be understood as a link.

[0011] It is proposed to determine at least one pair of reference vector and "measured" vector, which represents kinematic relationships of at least one of the joints and the two modules connected to the joint. Since measurements are performed only indirectly in this case, but the determination of these vectors can rather be considered as so-called virtual measurements, the at least one vector will also be referred to as a virtual vector. This vector pair then flows into the state estimation of the modules. The kinematic relationships can, in particular, be kinematic constraints, which represent the limitations of the components. For example, a joint can typically only rotate up to a maximum angle, and fixed modules cannot overlap.

[0012] It is particularly advantageous to determine vector pairs that represent the kinematic relationships. A first vector pair represents the kinematic relationship that the joint has the same joint axis from the perspective of each of the two modules connected to the joint. The expression "from the perspective of the..." indicates which coordinate system is used for analysis. In other words, the position and orientation of the joint or joint axis is independent of the module from which it is determined and is therefore the same for both modules. Consequently, the first vector also represents the kinematic relationship that the joint has the same joint axis from the perspective of each of the two modules.

[0013] A second vector pair represents the kinematic relationship that a measured joint angle, from the perspective of one module connected to the joint, specifies at least one axis of the other module connected to the joint. In other words, if the joint angle has been measured and the orientation of one module is known, the orientation of an axis of the other module is also known. The joint angle can be measured, for example, by a joint angle sensor.

[0014] Preferably, the at least one vector pair is incorporated into a fusion of the sensor data of the inertial sensor assigned to the respective module. Preferably, the two aforementioned vector pairs are determined for each inertial sensor and are incorporated into the associated filtering of the sensor data of this sensor. Particularly preferably, the fusion is performed by filtering. However, other sensor fusion methods can also be used, e.g., those based on graph analysis.

[0015] The modules are typically arranged along a kinematic chain, meaning the movement of one module depends on the movement of the previously arranged module. Preferably, the vectors are determined for one module at a time in sequence, starting with a first module connected to a fixed reference, particularly in the global coordinate system.

[0016] The computer program is configured to perform each step of the method, particularly when executed on a computer or control unit. It enables the method to be implemented in a conventional electronic control unit without requiring any structural modifications. For this purpose, it is stored on the machine-readable storage medium.

[0017] By loading the computer program onto a conventional electronic control unit, the electronic control unit is obtained, which is configured to incorporate the kinematic relationships into the state estimation.

[0018] The method is applied to a work machine with a multi-link, articulated arm. An example of such a work machine is an excavator with a bucket arm. The modules correspond to the links of the arm and can also correspond to other parts of the excavator, such as a superstructure. Short description of the drawings

[0019] An embodiment of the invention is illustrated in the drawings and explained in more detail in the following description. Figur 1 shows a work machine in the form of an excavator according to the prior art, on which the method according to the invention can be carried out. Figur 2 shows a flowchart of the state estimation method according to the state of the art. Figur 3 shows a flowchart of the state estimation method according to an embodiment of the invention. Embodiment of the invention

[0020] In the following, an embodiment of the method according to the invention for the state estimation of position and orientation of movable modules of an excavator 1 is described. Figur 1 The movable modules are considered to be an uppercarriage L1, which can be rotated to form an undercarriage U, a boom L2, an adjustable boom L3, a stick L4, and a bucket L5. The movable modules are connected to each other along a kinematic chain via joints J1, J2, J3, J4, and J5 and each have sensor units S1, S2, S3, S4, and S5, which include inertial sensors and magnetometers. A detailed description was given above in the 'State of the Art' section.

[0021] Figur 3 shows a flow chart of the embodiment of the method according to the invention. The same steps as in Figur 2 The methods shown according to the prior art are marked with the same reference numerals and their repeated description is omitted.

[0022] When using the notation, it is important to note that the right-hand index indicates between which modules or coordinate systems the movement takes place and the left-hand index indicates from which coordinate system the movement is viewed (the expression "from the perspective of ..." indicates which coordinate system is used for observation).

[0023] For each sensor S1, S2, S3, S4, S5, a determination 100, 110 of the vector pair Si n< mess,i , w n< ref,i and a determination 101, 111 of the second vector pair Si o< mess,i, W o< ref,i (Index i stands for any module with associated sensor), which are explained in detail below. These two vector pairs o and n replace or supplement the magnetic field vector measured in the state of the art. S m mess (see Figur 2 ). According to formula 1, the vectors flow Si n< mess, i , W n< ref,i and Si o< mess,i, W o< ref,i during filtering 21, 31 to determine the deviation or error between the virtual measurements and the expected reference values W n ref,i and W o< ref,i to minimize: n mess , i <none / > <mprescripts / > Si <none / > − R W , Si T ⋅ n ref , i <none / > <mprescripts / > w <none / > o mess , i <none / > <mprescripts / > Si <none / > − R W , Si T ⋅ o ref , i <none / > <mprescripts / > w <none / > Si n< mess,i and Si o< mess,i are the vectors measured (or virtually measured) in the sensor coordinate system, R W,Si indicates the orientation of the sensor Si relative to the global coordinate system W and W n< ref,i and W o< ref,i are the vectors as reference from the perspective of the global coordinate system W.

[0024] The following describes the determination 100, 101 of the vector pairs for the first sensor unit S1. The first sensor unit S1 is arranged on the upper carriage L1 and has inertial sensors as well as a joint angle sensor. The upper carriage L1 is connected to the undercarriage U along a kinematic chain via a first joint J1. The determination 100 of the first vector pair S 1 n< mess, 1, W n< ref, 1 as follows: The joint axis of the first joint J1, which connects the successive modules uppercarriage L1 and undercarriage U, can be specified directly relative to both coordinate systems of the modules. If the vector S 1 n< mess ,1 of the first vector pair for the first sensor S1 arranged on the superstructure L1 using the orientation between sensor 1 and superstructure L1 assumed to be known on the one hand and the vector W n< ref,i using the orientation estimate of the predecessor term L1 on the other hand, the two vectors thus determined should be identical or parallel due to the kinematic relationship as soon as they are transformed into a common coordinate system.

[0025] According to the Denavit-Hartenberg convention from Spong et al.'s paper "Robot Modeling and Control" (see above), the joint axis corresponds to the z-axis of the previous module. The previous module is defined here as the module that precedes and is directly connected to the current module along a kinematic chain emanating from the stationary module. In this case, the previous module is the undercarriage U: n <mprescripts / > U <none / > = e z

[0026] According to formula 3, a virtual measurement of the first vector in the coordinate system of the first sensor S1 arranged on the superstructure L1 can be carried out. n mess , 1 <none / > <mprescripts / > S 1 <none / > = R L 1 , S 1 T R U , L 1 T e z

[0027] R L 1, S 1 denotes a constant application parameter that represents the orientation of the first sensor S1 relative to the superstructure L1 and can be assumed to be known. R U,L 1 represents the rotational part of the transformation matrix TU,L1 between the upper carriage L1 and the undercarriage U (see Figur 2 ), the so-called A-matrix of the Denavit-Hartenberg convention. In the aforementioned paper by Spong et al., "Robot modeling and control" (Equation 3.10), this is defined as follows: A i = Rot z , θ i Trans z , d i Trans x , a i Rot x , α i

[0028] θ i refers to the variable joint angle and d i , α i and a i are constant kinematic joint parameters.

[0029] The following applies to the rotation part: R U , L 1 = Rot z , θ i Rot x , α i R L 1 , U = R U , L 1 T = Rot x , − α i Rot z − , θ i R L 1,U denotes the inverse or transposed rotation matrix of R U,L 1 .

[0030] Since a vector parallel to the z-axis is invariant with respect to rotation around the z-axis, formula 3 can be further simplified so that it only depends on constant parameters that determine either the orientation R L 1,S 1 of the first sensor S 1 with respect to the superstructure L1 or the kinematics of the joint, expressed by the parameter α i , regarding: n mess , 1 <none / > <mprescripts / > S 1 <none / > = R L 1 , S 1 T Rot x , − α i e z

[0031] At the same time, the first vector W n< ref as a reference from the perspective of the global coordinate system W according to formula 7 via the state estimation of the orientation of the previous link, i.e. the undercarriage U: n ref , 1 <none / > <mprescripts / > W <none / > = R W , U e z

[0032] It should be noted that neither the description of the first vector S 1 n< mess ,1 of the virtual measurement for the first sensor S1 nor for the description of the first vector W n< ref, 1 as a reference from the perspective of the global coordinate system W, the state estimation for the considered link, i.e. the superstructure L1, or for the first sensor S1 was used. The relationship expressed in formula 8 (corresponds to formula 1 above for the first sensor S1) can therefore be used in filtering 21 for the state estimation of the orientation R W , S 1 of the first sensor S1 to calculate the difference vector between the virtual measurement and the expected reference W n< ref, 1 to minimize: n mess , 1 <none / > <mprescripts / > s 1 <none / > − R W , S 1 T ⋅ n ref , 1 <none / > <mprescripts / > w <none / >

[0033] When determining 101 of the second vector pair S 1 o< mess, 1, W o< ref, 1 (further for the first sensor S1) proceeded as follows: According to formula 9, a virtual measurement of the second vector in the coordinate system of the first sensor S1, which is arranged on the superstructure L1, can be carried out. o mess , 1 <none / > <mprescripts / > S 1 <none / > = R S 1 , L 1 e x

[0034] R S 1, L1 denotes the inverse or transposed rotation matrix of R L 1, S 1 and is therefore also a constant application parameter that represents the orientation of the first sensor S1 relative to the superstructure L1 and can be assumed to be known.

[0035] At the same time, the second vector W o< ref, 1 as a reference from the perspective of the global coordinate system W according to formula 10 via the state estimation of the orientation of the previous link, i.e. the undercarriage U, and the rotation component R U,L 1 of the transformation matrix TU,L1 between the superstructure L1 and the undercarriage U: o ref , 1 <none / > <mprescripts / > W <none / > = R W , U R U , L 1 e x = R W , U Rot z , θ 1 Rot x , α 1 e x o ref , 1 <none / > <mprescripts / > W <none / > = R W , U Rot z , θ 1 e x

[0036] This vector depends only on the orientation of the undercarriage U with respect to the reference coordinate system W and the measured joint angle θ 1. More generally, this vector depends only on the orientation estimate of the predecessor module and the joint angle between the two modules.

[0037] It should also be noted that neither the description of the second vector S 1 o< mess, 1 for the first sensor S1 nor for the description of the second vector W o< ref, 1 as a reference from the perspective of the global coordinate system W, the state estimate for the module under consideration, i.e., the superstructure L1, or for the first sensor S1 was used. The relationship expressed in formula 11 (corresponds to formula 11 below for the first sensor S1) can therefore be incorporated into the filtering 21 for the first sensor in order to determine the difference vector between the virtual measurement and the expected reference W o< ref, 1 to minimize: o mess , 1 <none / > <mprescripts / > S 1 <none / > − R W , S 1 T ⋅ o ref , 1 <none / > <mprescripts / > W <none / >

[0038] The Figur 3 Determination 110 of the first vector pair S 2 n< mess ,2, W n< ref, 2 and determination 111 of the second vector pair S 2 o< mess, 2 , W o< ref, 2 for the second sensor S2 can be carried out in an analogous manner. This also applies to the determination of the vectors for the other sensors S3, S4, S5, which are shown in Figur 3 are not shown.

Claims

1. Method for state estimation of relative position and orientation of a plurality of modules (L1, L2, L3, L4, L5) of a common system, which are movable relative to one another via joints (J1, J2, J3, J4, J5), by means of inertial sensors (S1, S2, S3, S4, S5) arranged at the modules (L1, L2, L3, L4, L5), characterized in that at least one vector pair (Sinmess,i, Wnref,i or Siomess,i, Woref,i) is ascertained (100, 101, 110, 111), which represents kinematic relationships of at least one of the joints (J1, J2, J3, J4, J5) and the two modules (L1, L2, L3, L4, L5) connected to the joint (J1, J2, J3, J4, J5), and at least one vector pair (Sinmess,i), Wnref,i or Siomess,i, Woref,i) is included in a state estimation.

2. Method according to Claim 1, characterized by a first vector pair (Sinmess,i, Wnref,i), which represents the kinematic relationship that the joint (J1, J2, J3, J4, J5) has the same joint axis from the view of each of the two modules (L1, L2, L3, L4, L5) connected to the joint.

3. Method according to Claim 1 or 2, characterized by a second vector pair (Siomess,i Woref,i), which represents the kinematic relationship that a measured joint angle (θ) from the view of one of the links (L1, L2, L3, L4, L5) connected to the joint (J1, J2, J3, J4, J5) specifies at least one axis (ex) of the other link (L1, L2, L3, L4, L5) connected to the joint (J1, J2, J3, J4, J5).

4. Method according to any one of the preceding claims, characterized in that the at least one vector pair (Sinmess,i, Wnref,i or Siomess,i Woref,i) is included when fusing the sensor data of the inertial sensor (S1, S2, S3, S4, S5) assigned to the module (L1, L2, L3, L4, L5).

5. Method according to Claim 4, characterized in that the fusing of the sensor data is implemented by way of filtering (21, 31).

6. Method according to any one of Claims 1 to 5, characterized in that it is used in a work machine (1), which comprises a multi-link, articulated arm (2), wherein the modules (L2, L3, L4, L5) correspond to the links of the arm (2).

7. Computer program, set up to carry out each step of the method according to any one of Claims 1 to 6.

8. Machine-readable storage medium, on which a computer program according to Claim 7 is stored.

9. Electronic controller, set up to carry out, by means of a method according to any one of Claims 1 to 6, an estimation of relative position and orientation of a plurality of modules that are movable relative to one another.