Method for State Estimation of Position and Orientation of a Plurality of Movable Modules of a Common System

Abstract

A method is for performing state estimation of position and orientation of a plurality of modules of a common system, which are movable relative to one another via joints, using inertial sensors that are arranged on the modules. At least one vector pair is determined, which represents kinematic relationships of at least one of the joints and of two of the modules connected to the at least one joint, and which is included in the state estimation.

Description

The present invention relates to a method for state estimation of relative position and orientation of a plurality of modules of a common system, which are movable relative to one another by way of joints, in which kinematic relationships of the movable modules and the joints are included in the state estimation. Further, the invention relates to a computer program which carries out each step of the method when executed on a computing device, and a machine-readable storage medium that stores the computer program. Finally, the invention relates to an electronic controller which is set up to carry out the method according to the invention.

PRIOR ART

These days, automation is making rapid progress in the field of work machines. To automate the work machines and the tools thereof, it is necessary to know the relative position and the orientation of the work machine and the tools by way of a state estimation. The relative position and the orientation of a plurality of modules which are movable relative to one another and which are connected to one another by way of joints are ascertained by means of inertial sensors arranged at the modules.

As an example of a work machine, FIG. 1 illustrates an excavator 1 with a substructure U and a superstructure L1, wherein the superstructure L1 is rotatable relative to the substructure U in the horizontal by way of a first joint J1. A first sensor unit S1, which comprises inertial sensors which measure the linear acceleration and/or the rate of rotation of the superstructure in relation to a stationary reference coordinate system and a magnetometer which measures the Earth's magnetic field at this position, is arranged on the superstructure L1.

The excavator 1 comprises an excavator arm 2 with further links of boom L2, adjustable boom L3 and stick L4, and also a bucket 3 or L5. The boom L2 is connected via a second joint J2 to the superstructure L1 of the excavator 1, the adjustable boom L3 is connected via a third joint J3 to the boom L2, the stick L4 is connected via a fourth joint J4 to the adjustable boom L3 and the bucket L5 is connected via a fifth joint J5 to the stick L4. A sensor unit S1, S2, S3, S4, S5 is arranged on each link L1, L2, L3, L4, L5, said sensor unit comprising inertial sensors and a magnetometer. The inertial sensors are acceleration sensors and rate sensors, which measure the linear acceleration and the rate of rotation of the respective sensor in relation to a stationary reference coordinate system. Moreover, the sensor units S1, S2, S3, S4, S5 can comprise joint angle sensors, which each measure the joint angle of each joint J1, J2, J3, J4, J5. W labels a global coordinate system, in which the excavator 1 is situated.

A method known per se for state estimation is illustrated in FIG. 2 for the superstructure L1 and boom L2 and it is explained briefly below on the basis of this figure:

A transformation matrix T_WU and a unit quaternion q_WU derivable therefrom, which specify the relative position and the orientation of the substructure U in the global coordinate system W, are provided 10 for the substructure U. The sensor units S1, S2 (and also the further sensors S3, S4, S5 not shown in FIG. 2), more precisely the inertial sensors and the magnetometers, record 20, 30 measurement signals at the same time or in succession. The angular speed, denoted by ω, the acceleration, denoted by a, and the magnetic field, denoted by m, are recorded as measurement signals. The left-hand index in FIG. 2 specifies the reference coordinate system in which the respective measurement value was recorded. The measured angular speed _S1ω_mess, the measured acceleration siamess and the measured magnetic field _Sim_mess for the superstructure L1 pass through a filter 21, as a result of which, inter alia, a quaternion q_W,L1 which represents the rotation of the superstructure L1 in relation to the global coordinate system W and the estimated angular speed _L1ω_est of the superstructure L1 are ascertained. Now, by means of the quaternion of the superstructure L1 and the unit quaternion of the substructure U, a first joint angle θ₁ is ascertained 40 for the first joint J1 and, from this, the transformation matrix T_U,L1 for the transition between the substructure U and the superstructure L1 is subsequently ascertained 41. Finally, there is a matrix multiplication 42 of the transformation matrix T_W,U for the substructure U with the transformation matrix T_U,L1 for the transition between the substructure U and the superstructure L1, in order to obtain the transformation matrix T_W,L1 for the transition between the global coordinate system W and the superstructure L1.

Analogously, the measured angular speed _S2ω_mess, the measured acceleration _S2^a_mess and the measured magnetic field _S2m_mess for the boom L2 of the excavator arm 2 are recorded 30 by the second sensor S2 and pass through a filter 31, as a result of which, inter alia, a quaternion q_W,L2 which represents the rotation of the boom L2 in relation to the global coordinate system W and the estimated angular speed _L2ω_est of the boom L2 are ascertained. A second joint angle θ₂ is now ascertained 50 for the second joint J2 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. From this, the transformation matrix T_L1,L2 for the transition between the superstructure L1 and the boom L2 is subsequently ascertained 51 using the kinematic parameters, assumed as known, e.g., the

Denavit-Hartenberg parameters, of the articulated arm in a manner analogous to the so-called forward kinematics of robotic arms. Finally, there is a matrix manipulation 52 of the transformation matrix T_W,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, in order to obtain the transformation matrix T_W,L2 for the transition between the global coordinate system W and the boom L2.

The method can be continued in analogous fashion for the further links.

By way of example, a detailed description of the method for calculating the forward kinematics for joint angles for stationary machines is disclosed in the paper by Spong, Mark W., Seth Hutchinson, and Mathukumalli Vidyasagar, “Robot modeling and control”, vol. 3, New York: Wiley, 2006, which is referred to in this respect.

DISCLOSURE OF THE INVENTION

In the case of work machines which typically have a great mass of metal, the measurements of the magnetometers are altered by the great mass of metal and the changes in the magnetic field arising as a result, in such a way that they are frequently unusable or at least unreliable. It is also possible for the measurements of the individual magnetometers to be influenced differently such that state estimations for the respective modules or links drift apart and, as a result thereof, configurations of the orientation and/or the relative position of the modules or the links arise in the state estimates which are not possible according to the kinematics. By way of example, a state estimate for two modules interconnected by a joint specify different yaw angles in each case, even though this should be precluded by the kinematics. Within the scope of the present invention, a module can be understood to mean a link.

It is proposed to ascertain 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 at best indirect measurements are carried out in this case, the ascertainment of the aforementioned vectors rather being able to be considered to be so-called virtual measurements, the at least one vector is also referred to as a virtual vector. This vector pair is then included in the state estimate of the modules. In particular, the kinematic relationships can be kinematic constraints, which represent the restrictions on the components. Thus, a joint can typically only rotate up to a maximum angle and solid modules cannot overlap.

Vector pairs representing the kinematic relationships are particularly advantageously ascertained. Here, a first vector pair represents the kinematic relationship that the joint has the same joint axis from the view of each of the two modules connected to the joint. The expression “from the view of the . . . ” specifies the coordinate system used for the consideration. Expressed differently, the relative position and orientation of the joint or the joint axis is independent of the module from which it is ascertained and consequently the same for both modules. Accordingly, the first vector also represents the kinematic relationship that the joint has the same joint axis from the view of each of the two modules.

A second vector pair represents the kinematic relationship that a measured joint angle from the view of one module connected to the joint specifies at least an axis of the other module connected to the joint. Expressed differently, if the joint angle was measured and the orientation of the one module is known, the orientation of an axis of the other module is also known. By way of example, the joint angle can be measured by a joint angle sensor.

Preferably, the at least one vector pair is included when fusing the sensor data of the inertial sensor assigned to the respective module. Preferably, the two aforementioned vector pairs are ascertained for each inertial sensor and are included in the associated filtering of the sensor data of this sensor. Particularly preferably, the fusing is implemented by filtering. However, other sensor fusion methods can also be used, for example those based on the evaluation of graphs.

The modules are typically arranged along a kinematic chain, i.e., the movement of a module depends on the movement of the module arranged in front. Preferably, the ascertainment of the vectors for one module after another is carried out in succession, starting with a first module that is connected to a fixed reference, particularly in the global coordinate system.

The computer program is configured to carry out each step of the method, particularly if it is carried out on a computing device or controller. It facilitates the implementation of the method in a conventional electronic controller without the latter requiring structural modifications. To this end, it is stored on the machine-readable storage medium.

Uploading the computer program on a conventional electronic controller yields the electronic controller which is set up to allow the kinematic relationships to be included in the state estimation.

The method finds use in a work machine which has a multi-link, articulated arm. An example of such a work machine is an excavator with a bucket arm. In this case, the modules correspond to the links of the arm but can also correspond to further parts of the excavator, such as a superstructure for example.

BRIEF DESCRIPTION OF THE DRAWINGS

An exemplary embodiment of the invention is illustrated in the drawings and explained in more detail in the subsequent description.

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

FIG. 2 shows a flowchart of the method for state estimation according to the prior art.

FIG. 3 shows a flowchart of the method for state estimation according to an embodiment of the invention.

EXEMPLARY EMBODIMENT OF THE INVENTION

An embodiment of the method according to the invention for state estimation of relative position and orientation of movable modules of an excavator 1 of FIG. 1 is described below. A superstructure L1 rotatable relative to a substructure U, a boom L2, an adjustable boom L3, a stick L4 and a bucket L5 are considered to be movable modules. The movable modules are connected to one another along a kinematic chain via joints J1, J2, J3, J4, J5 and each have sensor units S1, S2, S3, S4, S5, which comprise inertial sensors and magnetometers. A detailed description was given above in the “prior art” section. FIG. 3 shows a flowchart of the embodiment of the method according to the invention. The same steps as in the method according to the prior art shown in FIG. 2 are denoted by the same reference signs and the renewed description thereof is dispensed with.

In relation to the notation, it should be observed that the right-hand index specifies the modules or coordinate systems between which the movement is implemented and the left-hand index specifies the coordinate system from which the movement is observed (the expression “from the view of the . . . ” specifies which coordinate system is used for observation purposes).

For each sensor S1, S2, S3, S4, S5 there is a respective ascertainment 100, 110 of the vector pair _sin_mess,i, _wn_ref,i and an ascertainment 101, 111 of the second vector pair _sio_mess,i, _wo_ref,i (the index i in this case represents any module with associated sensor), which are explained in detail below. These two vector pairs o and n replace or complement the magnetic field vector _sm_mess measured in the prior art (see FIG. 2). According to formula 1, the vectors _sin_mess,i, _wn_ref,i and _sio_mess,I, _wo_ref,i are included in the filtering 21, 31 in order to minimize the deviation or the error between the virtual measurements and the expected reference variables _wn_ref,i and _wo_ref,i:

_sin_mess,i−(R_W,Si)^T·_wn_ref,i

_Sio_mess,i−(R_W,Si)^T·_Wo_ref,i   (Formula 1)

_Sin_mess,i and _Sio_mess,i are the vectors measured (or virtually measured) in the sensor coordinate system, R_W,Si specifies the orientation of the sensor Si relative to the global coordinate system W and _Wn_ref,i and _Wo_ref,i are the vectors as the reference from the view of the global coordinate system W.

The ascertainment 100, 101 of the vector pairs for the first sensor unit S1 is described below. The first sensor unit S1 is arranged on the superstructure L1 and has inertial sensors and a joint angle sensor. The superstructure L1 is connected to the substructure U along a kinematic chain via a first joint J1. The following procedure is carried out when ascertaining 100 the first vector pair _Sin_mess,i, _Wn_ref,i:

The joint axis of the first joint J1, which connects the successive modules of superstructure L1 and substructure U, can be specified directly relative to the two coordinate systems of the modules. If, firstly, the vector _Sin_mess,i of the first vector pair for the first sensor Si arranged on the superstructure L1 is specified using the orientation, assumed as known, between sensor 1 and superstructure L1 and, secondly, the vector _Wn_ref,i is specified using the orientation estimation of the preceding link L1, the two vectors ascertained thus should be identical or parallel on account of the kinematic relationship as soon as they are transformed into a common coordinate system.

In accordance with the Denavit-Hartenberg convention from the paper by Spong et al. “Robot modeling and control” (see above), the joint axis corresponds to the z-axis of the preceding module. In this case, the preceding module should be considered the module which is arranged in front of the current module along a kinematic chain that starts at the stationary module and which is directly connected thereto. Thus, the preceding model is the substructure U in this case:

_Un=e_z   (Formula 2)

According to Formula 3, it is possible to carry out a virtual measurement of the first vector in the coordinate system of the first sensor S1, which is arranged on the superstructure L1.

_S1n_mess,1=(R_L1,S1)^T(R_U,L1)^Te_z   (Formula 3)

R_L1,S1 denotes a constant application parameter which represents the orientation of the first sensor S1 relative to the superstructure L1 and which can be assumed to be known. R_U,L1 represents the rotation component of the transformation matrix T_U,L1 between the superstructure L1 and the substructure U (see FIG. 2), i.e., the so-called A-matrix of the Denavit-Hartenberg convention. In the paper by Spong et al. “Robot modeling and control” (Equation 3.10), mentioned at the outset, this is defined as follows:

A_i=Rot_z,θiTrans_z,diTrans_x,aiRot_x,αi   (Formula 4)

Here, θ_i denotes the variable joint angle and d_i, α_i and a_i are constant kinematic joint parameters.

Therefore, the following applies to the rotation part:

R_U,L1=Rot_z,θiRot_x,αi

R_L1,U=R_U,L1^T=Rot_x,−αiRot_z−,θi   (Formula 5)

R_L1,U denotes the inverse or transposed rotation matrix of R_U,L1.

Since a vector parallel to the z-axis is invariant in relation to rotation about the z-axis, Formula 3 can be simplified further such that it only still depends on constant parameters which either relate to the orientation R_L1,S1 of the first sensor S₁ in relation to the superstructure L1 or the kinematics of the joint, expressed by the parameter α_i:

_S1n_mess,1(R_L1,S1)^TROt_x,−αie_z   (Formula 6)

At the same time, the first vector _Wn_ref can be specified as a reference from the view of the global coordinate system W as per Formula 7 by way of the state estimation of the orientation of the preceding link, i.e., the substructure U:

_Wn_ref,1=R_W,Ue_z   (Formula 7)

It should be observed that the state estimation for the considered link, i.e., the superstructure L1, or for the first sensor S1 was not used for the description of the first vector _S1n_mess,1 of the virtual measurement for the first sensor S₁ and not used for the description of the first vector _Wn_ref,1 as the reference from the view of the global coordinate system W. The relationship expressed in Formula 8 (corresponding to Formula 1 above for the first sensor S1) can therefore be included when filtering 21 for the state estimation of the orientation R_W,S1 of the first sensor S1 in order to minimize the difference vector between the virtual measurement and the expected reference _Wn_ref,1:

_S1n_mess,1−(R_W,S1)^T·_Wn_ref,1   (Formula 8)

When ascertaining 101 the second vector pair _S1o_mess,1, _Wo_ref,1 (still for the first sensor S1), the following procedure is carried out:

According to Formula 9, a virtual measurement of the second vector can be carried out in the coordinate system of the first sensor S1, which is arranged on the superstructure L1.

_S1o_mess,1=(R_S1,L1)e_x   (Formula 9)

R_S1,L1 denotes the inverse or transposed rotation matrix of R_L1,S1 and is consequently likewise a constant application parameter which represents the orientation of the first sensor S1 relative to the superstructure L1 and which can be assumed to be known.

At the same time, the second vector _Wo_ref,1 as a reference from the view of the global coordinate system W as per Formula 10 can be specified by way of the state estimation of the orientation of the preceding link, i.e., the substructure U, and the rotation component R_U,L1 of the transformation matrix T_U,L1 between the superstructure L1 and the substructure U:

_Wo_ref,1=R_W,UR_U,L1e_x=R_W,URot_z,θ1Rot_x,α1e_x

_Wo_ref,1=R_W,URot_z,θ1e_x   (Formula 10)

This vector only depends on the orientation of the substructure U in relation to the reference coordinate system W and the measured joint angle θ₁. Expressed more generally, this vector only depends on the orientation estimation of the preceding module and the joint angle between the two modules.

It should also be observed here that the state estimation for the considered module, i.e., the superstructure L1, or for the first sensor S1 was not used for the description of the second vector _S1o_mess,1 for the first sensor S1 and not used for the description of the second vector _Wo_ref,1 as a reference from the view of the global coordinate system W. The relationship expressed in Formula 11 (corresponding to Formula 11 for the first sensor S1 below) can therefore be included in the filtering 21 for the first sensor in order to minimize the difference vector between the virtual measurement and the expected reference _Wo_ref,1:

_S1o_mass,1−(R_W,S1)^T·_Wo_ref,1   (Formula 11)

The ascertainment 110 of the first vector pair _S2n_mess,2, _Wn_ref,2 and the ascertainment 111 of the second vector pair _fo_mess,2, _Wo_ref,2 for the second sensor S2, illustrated in FIG. 3, can be carried out in analogous fashion. This also applies to the ascertainment of the vectors for the other sensors S3, S4, S5, which are not shown in FIG. 3 for reasons of clarity.

Claims

1. A method for state estimation of relative position and orientation of a plurality of modules of a common system, comprising:

moving the modules of the plurality of modules relative to one another via a plurality of joints connecting the modules;

sensing the movement of the modules using inertial sensors arranged at the modules;

ascertaining at least one vector pair, which represents a kinematic relationship of at least one joint of the plurality of joints and two of the modules connected to the at least one joint; and

including the at least one ascertained vector pair in the state estimation.

2. The method as claimed in claim 1, wherein the at least one ascertained vector pair, which represents the kinematic relationship of the at least one joint, corresponds to a same joint axis from a view of each of the two modules connected to the at least one joint.

3. The method as claimed in claim 1, further comprising:

ascertaining at least one second vector pair, which represents a further kinematic relationship of a measured joint angle from a view of a selected module of the two modules connected to the at least one joint,

wherein the at least one second vector pair specifies at least one axis of the selected module.

4. The method as claimed in claim 1, further comprising:

including the at least one vector pair when fusing sensor data of the inertial sensors arranged at the modules.

5. The method as claimed in claim 4, further comprising:

fusing the sensor data by filtering.

6. The method as claimed in claim 1, wherein a computer program is configured to carry out the method.

7. The method as claimed in claim 6, wherein the computer program is stored on a machine-readable storage medium.

8. The method as claimed in claim 1, wherein an electronic controller is configured to estimate a relative position and orientation of the plurality of modules using the method.

9. The method as claimed in claim 1, wherein:

the method is used in a work machine, which comprises a multi-link, articulated arm, and

the modules correspond to links of the multi-link, articulated arm.