CN113330167B

CN113330167B - Method for state estimation of the position and orientation of a plurality of movable modules of a common system

Info

Publication number: CN113330167B
Application number: CN202080011335.7A
Authority: CN
Inventors: A·洛佩兹潘普洛纳; S·舍雷尔; D·奈尔
Original assignee: Robert Bosch GmbH
Current assignee: Robert Bosch GmbH
Priority date: 2019-01-29
Filing date: 2020-01-22
Publication date: 2022-11-22
Anticipated expiration: 2040-01-22
Also published as: WO2020156897A1; JP2022523713A; EP3918138A1; JP7250940B2; US20220162834A1; DE102019201091A1; CN113330167A

Abstract

The invention relates to a method for the state estimation of the positions and orientations of a plurality of modules of a common system, which are movable relative to one another by means of joints (J1, J2), by means of inertial sensors (S1, S2) which are arranged on the modules. At least one vector pair (100, 101, 110, 111) is determined (100, 101, 110, 111) (ii)

Or

) The vector pair represents the kinematic relationship of at least one of the joints (J1, J2) and of two modules connected to the joint (J1, J2), and is included in the state estimation.

Description

Method for state estimation of the position and orientation of a plurality of movable modules of a common system

Technical Field

The invention relates to a method for state estimation of the position and orientation (Orientierung) of a plurality of modules of a common system that can be moved relative to one another by means of joints, in which method the kinematic relationships of the modules that can be moved and of the joints are included in the state estimation. Furthermore, the invention relates to a computer program and a machine-readable storage medium storing the computer program, which implement each step of the method when the computer program runs on a computing device. Finally, the invention relates to an electronic control unit which is designed to carry out the method according to the invention.

Background

Automation is currently rapidly advancing in the field of work machines. In order to automate a work machine and its tools, it is necessary to know the position and orientation of the work machine and of the tools by means of state estimation. The position and orientation of a plurality of modules which are connected to one another by means of joints and can be moved relative to one another is determined by means of inertial sensors arranged on the modules.

Fig. 1 shows an excavator 1 as an embodiment for a work machine, having a substructure U and an superstructure L1, wherein the superstructure L1 can be rotated relative to the substructure U by means of a first joint J1 along a horizontal line. At the superstructure L1, a first sensor unit S1 is arranged, which has an inertial sensor for measuring the linear acceleration and/or the rotation rate of the superstructure relative to a positionally fixed reference coordinate system, and a magnetometer for measuring the geomagnetic field at this position. The excavator 1 has an excavator arm 2 with other components, a boom arm L2, an adjusting boom L3 and a stick L4, and a bucket 3 or L5. The boom L2 is connected to an upper structure L1 of the excavator 1 through a second joint J2, the adjusting boom L3 is connected to the boom L2 through a third joint J3, the arm L4 is connected to the adjusting boom L3 through a fourth joint J4, and the bucket L5 is connected to the arm L4 through a fifth joint J5. At each component L1, L2, L3, L4, L5, a sensor unit S1, S2, S3, S4, S5 is arranged, which has an inertial sensor and a magnetometer, respectively. The inertial sensors are acceleration sensors and rotation rate sensors, which measure the linear acceleration and rotation rate of the respective sensor relative to a stationary reference coordinate system. Furthermore, the sensor units S1, S2, S3, S4, S5 can have joint angle sensors that measure the joint angle of each joint J1, J2, J3, J4, J5, respectively. The global coordinate system in which the excavator 1 is located is marked with W.

A method known per se for the state estimation of the superstructure L1 and the cantilever L2 is shown in fig. 2 and is briefly explained below on the basis of this figure.

Providing 10 transformation matrices for infrastructure U

And unit quaternion derivable therefrom

They indicate the position and orientation of the infrastructure U in the global coordinate system W. The sensor units S1, S2 (further sensors S3, S4, S5, which are also not shown in fig. 2), more precisely the inertial sensor and the magnetometer,

record

20, 30 measurement signals simultaneously or successively. The angular velocity marked with ω, the acceleration marked with a and the magnetic field marked with m are recorded as measurement signals. The index on the left indicates in fig. 2 in which reference coordinate system the respective measured value is recorded. Angular velocity measured for the superstructure L1

Measured acceleration

And the measured magnetic field

Passes through a filter 21, whereby the quaternion

In particular, represents the rotation of the superstructure L1 relative to the global coordinate system W, and the estimated angular velocity of the superstructure L1 is determined

. For the first joint J1, quaternion by means of the superstructure L1 is now used

And unit quaternion of the substructure U to determine 40 a first joint angle

And then subsequently determining 41 a transformation matrix for the transition between the substructure U and the superstructure L1 therefrom

. Finally, a transformation matrix for the infrastructure U is performed

And a transformation matrix for the transition between the substructure U and the superstructure L1

In order to obtain a transformation matrix for the transition between the global coordinate system W and the upper structure L1

。

In a similar manner, the measured angular velocity for the jib L2 of the excavator arm 2

Measured acceleration

And the measured magnetic field

Is registered 30 by the second sensor S2 and passes through a filter 31, whereby inter alia a quaternion representing the rotation of the cantilever L2 with respect to the global coordinate system W is determined

And estimated angular velocity of the cantilever L2

. For the second joint J2, a quaternion by means of the cantilever L2 is now used

And by means of quaternion of L1 of the superstructure

Measuring 50 second Joint Angle

. Thus, the transformation matrix for the transition between the superstructure L1 and the cantilever L2 is subsequently determined 51 with a so-called forward kinematics similar to a robotic arm, using the assumed known kinematic parameters of the articulated arm, like for example the parameters of delavirt-Ha Tengba lattice (Denavit-Hartenberg)

. Finally, a transformation matrix for the transition between the global coordinate system W and the superstructure L1 is performed

And a transformation matrix for the transition between the superstructure L1 and the cantilever L2

In order to obtain a transformation matrix for the transition between the global coordinate system W and the cantilever L2

。

The method can be continued in a similar manner for other components.

For example, in spongo, mark W, sahnkinson, and Ma Tuku Ma Liwei disosaka "robots model and control" volume 3, new york: a detailed description of a method for calculating forward kinematics from joint angles for stationary machines is given in the paper of Venue, 2006 (Spong, mark W., seth Hutchinson, and Mathukumuli Vidyasagar, "Robot Modeling and Control", vol. 3.New York: wiley, 2006), to which reference is made for this purpose.

Disclosure of Invention

For work machines which usually have a large amount of metal, the measurement of the magnetometer is changed due to the large amount of metal and the resulting change in the magnetic field in such a way that the measurement is often unusable or at least unreliable. The measurements of the individual magnetometers can also be influenced differently, so that the state estimates for the respective modules or components drift away from one another, and thus a configuration of the orientation and/or position of the modules or components results at the time of the state estimation, which is not feasible according to kinematics. For example, the state estimation of two modules connected by a joint each indicates a different deflection angle, although this should be excluded by kinematics. A module can be understood as a component within the framework of the invention.

It proposes that: at least one pair of reference vectors and a "measured" vector is determined which represents the kinematic relationship of at least one of the joints and two modules connected to the joint. Since the measurement is carried out at most indirectly here, and the determination of the mentioned vectors can be regarded as what is known as a virtual measurement, at least one vector is also referred to as a virtual vector. This vector pair is then incorporated into the state estimate of the module. Kinematic relationships can be, in particular, kinematic constraints (english), which represent the limits of a component. The joint can therefore usually only be rotated up to a maximum angle, and the fixed modules can not overlap.

It is particularly advantageous to determine pairs of vectors representing kinematic relationships. The first vector pair represents a kinematic relationship here: the joint has the same joint axis from the perspective of each of the two modules connected to the joint. The expression "from what perspective" indicates which coordinate system is considered for the observation. In other words, the position and orientation of the joints or joint axes are not dependent on which module they are determined from, and are therefore the same for both modules. The kinematic relationship is therefore also represented by the first vector: the joint has the same joint axis from the perspective of each of the two modules.

The second vector pair represents such a kinematic relationship: the measured joint angle specifies at least one axis of the other module connected to the joint from the perspective of the module connected to the joint. In other words, if the joint angle is measured and the orientation of one module is known, then the orientation of the axis of the other module is also known. The joint angle can be measured, for example, by a joint angle sensor.

Preferably, at least one vector pair is included in the merging of the sensor data of the inertial sensors assigned to the respective module. Preferably, two of the aforementioned vector pairs are determined for each inertial sensor and are included in the relevant filtering of the sensor data of this sensor. Particularly preferably, the combining is performed by filtering. However, other sensor integration methods, such as those based on analysis of graphs, can also be used.

The modules are usually arranged along a kinematic chain, that is to say the movement of the modules is dependent on the movement of the modules arranged above. Preferably, the measurement of the vectors for the modules is carried out in a sequential manner, starting from a first module which is connected to a fixed reference, in particular in a global coordinate system.

A computer program is provided for carrying out each step of the method, in particular when the computer program is executed on a computing device or controller. The implementation of the method in a conventional electronic control unit can be carried out without structural changes being necessary here. For this purpose, the computer program is stored on a machine-readable storage medium.

Such an electronic control unit is obtained by loading a computer program onto a conventional electronic control unit, which is designed to include the kinematic relationships in the state estimation.

The method is used in a work machine having a multi-part, articulated arm. An example for such a work machine is an excavator with an excavator arm. The modules correspond to the components of the arm, and can additionally also correspond to other components of the excavator, such as, for example, the superstructure.

Drawings

Embodiments of the invention are illustrated in the drawings and are explained in more detail in the following description.

Fig. 1 shows a work machine according to the prior art in the form of an excavator, at which a method according to the invention can be carried out;

FIG. 2 shows a flow diagram of a method for state estimation according to the prior art;

fig. 3 shows a flow chart of a method for state estimation according to an embodiment of the invention.

Detailed Description

In the following, embodiments of the method according to the invention for state estimation of the position and orientation of a movable module from the excavator 1 of fig. 1 are explained. The superstructure L1, the jib L2, the adjusting jib L3, the arm L4 and the bucket L5, which are rotatable relative to the substructure U, are considered as movable modules. The movable modules are connected to each other along a kinematic chain by joints J1, J2, J3, J4, J5 and each have a sensor unit S1, S2, S3, S4, S5, which includes an inertial sensor and a magnetometer. A detailed description is given above in the "prior art" section. Fig. 3 shows a flow chart of an embodiment of the method according to the invention. As in the method according to the prior art shown in fig. 2, like steps are denoted by like reference numerals, and a renewed description is omitted in this respect.

It is necessary to note for the marking that: the index on the right indicates between which module or coordinate system the motion is made, and the index on the left indicates from which coordinate system the motion is observed (the expression "from what perspective" indicates which coordinate system is considered for observation).

Vector pairs are generated for each sensor S1, S2, S3, S4, S5

Is determined 100, 110 and a second vector pair

Of the sensors 101, 111 (index i here denotes any module with associated sensors), which vector pair is explained in detail below. Two vector pairsoAndninstead of, or in addition to, the magnetic field vector measured in the prior art

(see FIG. 2). Vector according to equation 1

And

the

filtering

21, 31 is included in order to minimize the measured and expected reference quantities in the virtual

And

deviation or error between:

(equation 1).

And

is a vector measured (or virtually measured) in the sensor coordinate system,

indicates the orientation of the sensor Si relative to the global coordinate system W, and

and

is a vector that is considered as a reference from the perspective of the global coordinate system W.

The

determination

100, 101 of the vector pair for the first sensor unit S1 is explained below. The first sensor unit S1 is arranged at the upper structure L1 and has an inertial sensor and a joint angle sensor. The superstructure L1 is connected to the substructure U along a kinematic chain by means of a first joint J1. For the first vector pair

The measurement 100 of (b) is performed as follows.

The joint axis of the first joint J1 connecting the following module superstructure L1 with the substructure U can be directly assigned with respect to the two coordinate systems of the module. If, on the one hand, the vector of the first vector pair for the first sensor S1 arranged on the superstructure L1 is specified using an assumed known orientation between the sensor 1 and the superstructure L1

And on the other hand specifying the vector using the orientation estimation of the front member L1

Then, once they have been transformed into a common coordinate system, the two vectors thus determined should be identical or parallel due to kinematic relationships.

According to the Denavitt-Ha Tengba lattice reduction from the St.Pont et al "robot modeling and control" (see above), the joint axis corresponds to the z-axis of the previous module. In this case, such a module is considered to be a previous module, which is arranged along the kinematic chain starting from the motionless module in front of the current module and is connected directly to this module. In this case, the previous module is the infrastructure U:

(equation 2).

A virtual measurement of the first vector in the coordinate system of the first sensor S1 arranged at the upper structure L1 can be performed according to equation 3:

(equation 3).

Referred to as a constant application parameter, which represents the orientation of the first sensor S1 relative to the superstructure L1 and can be assumed to be known.

Representing a transformation matrix between said upper structure L1 and said lower structure U

The rotational component of (see fig. 2), the so-called a-matrix of the dernavitt-Ha Teng berg convention. In the paper by Sponger et al "robot modelling and control" mentioned at the beginning (equation 3.10), the matrix is defined as follows:

(equation 4).

In this case, the amount of the solvent to be used,

is referred to as a varying joint angle, and

、

and

are constant kinematic joint parameters.

Therefore for the rotational component:

(equation 5).

Is called as

The inverse of or the transposed rotation matrix.

Since the vector parallel to the z-axis is invariant with respect to rotation about the z-axis, equation 3 can be further simplified such that it depends only on constant parameters which are either related to the first sensor

With respect to the orientation of the superstructure L1

Either the passing parameters are involved

To indicate the jointKinematics:

(equation 6).

At the same time, the first vector as a reference from the perspective of the global coordinate system W

It can be specified by a state estimation of the orientation of the previous component, i.e. the U of the substructure, according to equation 7:

(equation 7).

It is necessary to note that: neither is the first vector of the virtual measurement for the first sensor S1

Also, the first vector as a reference from the perspective of the global coordinate system W is not considered

The description of (1) uses state estimates for the observed component, i.e. the superstructure L1 or for the first sensor S1. Therefore, the relationship represented in equation 8 (corresponding to equation 1 above for the first sensor S1) can be used in the orientation for the first sensor S1

Is included in the filtering 21 of the state estimate in order to minimize the difference between the virtual measurement and the expected reference

The difference vector between:

(equation 8).

For determining 101 a second vector pair

In this case (also for the first sensor S1), the following procedure is carried out.

A virtual measurement of the second vector in the coordinate system of the first sensor S1 arranged at the upper structure L1 can be performed according to equation 9:

(equation 9).

Is called as

And is thus likewise a constant application parameter which represents the orientation of the first sensor S1 relative to the superstructure L1 and can be assumed to be known.

At the same time, the second vector, which is taken as a reference from the perspective of the global coordinate system W

The state estimation of the previous components, i.e. the orientation of the lower structure U, and the transformation matrix between the upper structure L1 and the lower structure U can be carried out according to equation 10

Rotational component of

To specify:

(equation 10).

The vector depends only on the orientation of the substructure U with respect to the reference coordinate system W and on the measured joint angle

. More generally, the vector depends only on the orientation estimate of the previous module and the joint angle between the two modules.

It is also necessary to note here that: neither for the second vector for the first sensor S1

For the second vector which is taken as a reference from the perspective of the global coordinate system W, the description of

The description of (1) uses state estimates for the module under observation, i.e. the superstructure L1 or for the first sensor S1. Therefore, the relationship represented in equation 11 (corresponding to equation 11 for the first sensor S1 below) can be included in the filtering 21 for the first sensor in order to minimize the difference between the virtual measurement and the expected reference

The difference vector between:

(equation 11).

First vector pair

The measurement 110 and the second vector pair shown in FIG. 3

The determination 111 of (d) can be performed in a similar manner for the second sensor S2.This also applies to the determination of the vectors for the further sensors S3, S4, S5, which are not shown in fig. 3 for the sake of clarity.

Claims

1. Method for the state estimation of the position and orientation of a plurality of modules (L1, L2, L3, L4, L5) of a common system, which modules are movable relative to each other by means of 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,

measurement (100, 101, 110 111) first vector pair (

) Or a second vector pair (

) Represents the kinematic relationship of at least one of the joints (J1, J2, J3, J4, J5) and of two modules (L1, L2, L3, L4, L5) connected to the joint (J1, J2, J3, J4, J5), and a first vector pair (c: (b) ((b))

) Or a second vector pair (

) Is included in the state estimate.

2. The method of claim 1, characterized by a first pair of vectors (c), (d)

) The first vector pair represents a kinematic relationship: the joints (J1, J2, J3, J4, J5) are formed from two modules (L1, L2, L3, L4, L5) connected to the jointsEach module has the same joint axis from the perspective of the module.

3. The method according to claim 1 or 2, characterized in that the second vector pair (c) is

) The second vector pair represents a kinematic relationship: measured joint angle (

) At least one axis (L1, L2, L3, L4, L5) of a further module (L1, L2, L3, L4, L5) connected to the joint (J1, J2, J3, J4, J5) is predefined from the point of view of a component of the module (L1, L2, L3, L4, L5) connected to the joint (J1, J2, J3, J4, J5)

）。

4. Method according to claim 1 or 2, characterized in that a first vector pair (S), (S3, S4, S5) is included in merging the sensor data of the inertial sensors (S1, S2, S3, S4, S5) assigned to the modules (L1, L2, L3, L4, L5)

) Or a second vector pair (

) At least one vector pair.

5. Method according to claim 4, characterized in that the combination of sensor data is performed by filtering (21, 31).

6. A machine readable storage medium, on which a computer program is stored, the computer program being arranged to perform each step of the method according to any one of claims 1 to 5.

7. An electronic controller configured to perform the estimation of the position and orientation of a plurality of modules movable relative to each other by means of the method according to any one of claims 1 to 5.

8. Use of the method according to one of claims 1 to 5 in a work machine (1) having a multi-component, articulated arm (2), wherein the modules (L2, L3, L4, L5) correspond to components of the arm (2).