JP7250940B2 - Method for state estimation of the position and orientation of multiple movable modules of a common system - Google Patents

Description

The present invention relates to a method for state estimation of the position and orientation of a plurality of modules mutually movable via joints of a common system, wherein the kinematic relationships of the movable modules and the joints are introduced into the state estimation. . Furthermore, the invention relates to a computer program for performing the steps of the method when running on a computer, and a machine-readable storage medium on which the computer program is stored. Finally, the invention relates to an electronic control device arranged to implement the method according to the invention.

PRIOR ART Today, automation in the field of working machines is progressing rapidly. In order to automate a work machine and its tools, state estimation needs to know the position and orientation of the work machine and its tools. The position and orientation of multiple mutually movable modules interconnected via joints are determined using multiple inertial sensors located on the modules.

FIG. 1 shows, as an example of a working machine, an excavator 1 comprising a lower structure U and an upper vehicle body L1, wherein the upper vehicle body L1 is connected to the lower vehicle body via a first joint J1. It is rotatable in the horizontal direction with respect to U. Arranged on the superstructure L1 is a first sensor unit S1 which measures the linear acceleration and/or yaw rate of the superstructure relative to a stationary reference coordinate system. It has a sensor and a magnetometer that measures the geomagnetism at that location. The excavator 1 comprises an excavator arm 2 with a further link boom L2, an adjustable boom L3 and a stem L4 and a bucket 3 or L5. The boom L2 is connected to the upper body L1 of the excavator 1 via a second joint J2, the adjusting boom L3 is connected to the boom L2 via a third joint J3, and the stem L4 is connected to the fourth joint. and the bucket L5 is connected to the stem L4 via a fifth joint J5. A sensor unit S1, S2, S3, S4, S5 having an inertial sensor and a magnetometer is arranged on each link L1, L2, L3, L4, L5, respectively. Inertial sensors are acceleration and yaw rate sensors that measure the linear acceleration and yaw rate of each sensor relative to a fixed reference frame. Furthermore, the sensor units S1, S2, S3, S4, S5 may each have a joint angle sensor for measuring the joint angle of each joint J1, J2, J3, J4, J5. The global coordinate system in which the excavator 1 resides is designated by W.

A method known per se for estimating the state of the upper body L1 and the boom L2 is shown in FIG. 2 and will be briefly explained below on the basis of this figure.

For the underbody U, a transformation matrix T _wu and a derivable unit quaternion q _wu are provided (10), which describe the position and orientation of the underbody U in the global coordinate system W. Simultaneously or one after the other, sensor units S1, S2 (also further sensors S3, S4, S5, but not shown in FIG. 2), more precisely inertial sensors and magnetometers, receive measurement signals (20 , 30). As measurement signals, an angular velocity, denoted by ω, an acceleration, denoted by a, and a magnetic field, denoted by m, are received. In FIG. 2, the index on the left indicates in which reference coordinate system each measurement was received. For the upper body L1, the measured angular velocity _S1 ω _mess , the measured acceleration _S1 a _mess and the measured magnetic field _S1 m _mess are passed through a filter 21, whereby in particular the upper body with respect to the global coordinate system W A quaternion q _{W, L1 representing the turn of L1} and an estimated angular velocity _L1 ω _est of the upper vehicle body L1 are obtained. For the first joint J1, the first joint angle θ ₁ is obtained (40) using the quaternion _qW,L1 of the upper body L1 and the unit quaternion of the lower body U (40), and then , the transformation matrix TU _,L1 for the transition between the lower vehicle body U and the upper vehicle body L1 is subsequently determined (41). Finally, to obtain the transformation matrix _TW,L1 for the transition between the global coordinate system W and the upper vehicle body L1, the transformation matrix _TW,U for the lower vehicle body U, the lower vehicle body U and the upper vehicle body L1 A matrix multiplication 42 is performed with the transformation matrix TU _,L1 for the transition between .

In a similar manner, for the boom L2 of the excavator arm 2, the measured angular velocity _S2 ω _mess , the measured acceleration _S2 a _mess and the measured magnetic field _S2 m _mess are received from the second sensor S2 ( 30), passed through a filter 31, which inter alia determines the quaternion q _W,L2 representing the swivel of the boom L2 with respect to the global coordinate system W and the estimated angular velocity _L2 ω _est of the boom L2. For the second joint J2, a second joint angle _θ2 is determined (50), here using the quaternion qW _,L2 of the boom L2 and the quaternion _qW,L1 of the upper body L1. . From there, similarly to the so-called forward kinematics of the robot arm, using assumed known kinematic parameters, for example the Denavit-Hartenberg parameters, of the articulated arm, between the upper body L1 and the boom L2 A transformation matrix T _L1,L2 for the transition of is determined (51). Finally, to obtain the transformation matrix TW _,L2 for the transition between the global coordinate system W and the boom L2, the transformation matrix TW,L1 for the transition between the global coordinate system W and the upperbody _L1 and , with the transformation matrices T _L1, L2 for the transition between the upper body L1 and the boom L2.

For further links, the method can continue further as well.

For a detailed description of the method for computing forward kinematics from joint angles for stationary machines, see, for example, the paper by Spong, Mark W., Seth Hutchinson and Mathukumalli Vidyasagar, Robot modeling and control, Vol. York: Wiley, 2006", to which reference is made to this extent.

Paper "Robot modeling and control, Vol.3. New York:Wiley,2006" by Spong, Mark W., Seth Hutchinson and Mathukumalli Vidyasagar

DISCLOSURE OF THE INVENTION Typically, for work machines having heavy metals, the magnetometer measurements are often unusable due to the heavy metals and the magnetic fields generated thereby. or at least change to such an extent that it becomes unreliable. It should also be noted that the individual magnetometer measurements may be affected differently, causing the state estimates of each module or link to disjoint from each other, making state estimation kinematically impossible. Various module or link orientations and/or positional configurations occur. As an example, the state estimates for two modules connected via one joint show different yaw angles, even though kinematically they should be excluded. Within the framework of the present invention, a module can be understood to mean a link.

It is proposed to determine at least one pair consisting of a reference vector and a "measured" vector, the at least one pair representing the kinematic relationship between at least one of the joints and the two modules connected to the joint. represents At least one vector is also referred to as a virtual vector, since here the measurements can be carried out at most indirectly, and the determination of the aforementioned vectors can even be regarded as a so-called virtual measurement. This vector pair is therefore introduced into the module's state estimation. The kinematic relationships may in particular be kinematic constraints that describe the limits of the components. As such, joints can typically only be rotated to a maximum angle and fixation modules cannot be stacked.

Particularly preferably, vector pairs representing kinematic relationships are determined. The first vector pair here represents the kinematic relationship that the joint has the same joint axis from each point of view of the two modules connected to it. The phrase "from the point of view of" indicates which coordinate system is used for observation. In other words, the position and orientation of the joint or joint axis does not depend on which module it is derived from, and is therefore identical for both modules. Thus, the first vector also describes the kinematic relationship that the joint has the same joint axis from each point of view of the two modules.

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

Preferably, at least one vector pair is introduced in the fusion of the sensor data of the inertial sensors assigned to each module. Preferably, the above two vector pairs are determined for each inertial sensor and introduced in the associated filtering of the sensor data for that sensor. Particularly preferably, the fusion is done by filtering. However, other sensor fusion methods, such as those based on graph evaluation, can also be used.

These modules are typically arranged along a kinematic chain, ie the motion of a module depends on the motion of the module placed before it. Preferably, the determination of the vectors for the modules is carried out sequentially, especially in the global coordinate system, starting with the first module connected to a fixed reference.

The computer program is especially designed for implementing the steps of the method when it is run on a computer or controller. It should be noted that implementation of the method in conventional electronic control units is now possible without having to implement structural changes. To this end, the computer program is stored on a machine-readable storage medium.

By loading the computer program into a conventional electronic controller, an electronic controller configured to introduce kinematic relationships in state estimation is obtained.

The method is used on work machines having articulated arms. 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 they can also correspond to further parts of the excavator, for example the superstructure.

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

1 shows a working machine in the form of an excavator according to the prior art on which the method according to the invention can be implemented; FIG. 1 is a flowchart of a method for state estimation according to the prior art; 4 is a flowchart of a method for state estimation according to an embodiment of the invention;

Embodiment of the Invention In the following, an embodiment of the method according to the invention for state estimation of the position and orientation of the mobile module of the excavator 1 of FIG. 1 is described. The upper car body L1, the boom L2, the adjusting boom L3, the stem L4 and the bucket L5, which are rotatable relative to the lower car body U, are considered movable modules. These moveable modules are interconnected via joints J1, J2, J3, J4, J5 along a kinematic chain and sensor units S1, S2 each containing an inertial sensor and a magnetometer. , S3, S4, S5. A detailed description is given in the "Prior Art" section above. FIG. 3 shows a flowchart of an embodiment of the method according to the invention. The same steps as in the prior art method shown in FIG. 2 are denoted by the same reference numerals and their new description is omitted.

Note that in the notation, the index on the right indicates between which modules or coordinate systems the motion was made, and the index on the left indicates from which coordinate system the motion is observed ("of The expression "from the point of view" indicates which coordinate system is used for observation).

For each sensor S1, S2, S3, S4, S5, respectively, determine 100, 110 the vector pair _{Si mess,i} , _{Wnref ,i} and the second vector pair _{Si mess,i} , _Wo Steps 101, 111 of determining _ref,i are performed (index i here representing any module with an associated sensor), which are described in detail below. These two vector pairs o and n replace or supplement the magnetic field vector _S m _mess measured by the prior art (see FIG. 2). According to Equation ₁ , the vectors _{Sin mess,i} , _{Wnref ,i} and _{Siomess ,i} , _{Woref ,i} are the virtual measurements and the expected reference variables _{Wnref ,i} and _{Woref , i are introduced in filtering 21, 31 to minimize deviations or errors between i} .
_Si n _mess,i −(R _W,Si ) ^T · _W n _ref,i
(Formula 1) _Si o _mess,i −(R _{W, Si} ) ^T · _W o _{ref, i}
where _Si n _mess,i and _Si o _mess,i are the vectors measured (or virtually measured) in the sensor coordinate system, and R _W,Si are the vectors of the sensor Si relative to the global coordinate system W. Denoting the orientation, _W n _ref,i and _W o _ref,i are vectors as references from the point of view of the global coordinate system W;

In the following, the steps 100, 101 of determining the vector pair for the first sensor unit S1 are described. This first sensor unit S1 is arranged on the upper vehicle body L1 and has an inertial sensor and a joint angle sensor. The upper vehicle body L1 is connected to the lower vehicle body U via a first joint J1 along a kinematic linkage. Note that the step 100 of determining the first vector pair _S1 n _mess,1 , _W n _ref,1 is as follows.

The joint axis of the first joint J1 connecting the upper structure L1 and the lower structure U of successive modules can be specified directly with respect to both coordinate systems of the module. Using the vector _S1 n _mess,1 of the first vector pair for the first sensor S1 located on the upper body L1, on the one hand the known and assumed orientation between the sensor 1 and the upper body L1 and, on the other hand, using the orientation estimate of the preceding link L1 to specify the vector _W n _ref,i , the two vectors so determined are transformed into their common coordinate system should be the same or parallel based on the kinematic relationship immediately.

According to the Denavit-Hartenberg notation from Spong et al.'s paper "Robot modeling and control" (see above), the joint axis corresponds to the z-axis of the preceding module. A preceding module can here be regarded as a module that is positioned before and directly connected to the current module along the kinematic chain starting from the inactive module. In this case, the preceding module is the underbody U.
(Formula 2) _U n = e _z

ここで、θ_ｉは、可変関節角度を表し、ｄ_ｉ、α_ｉ及びａ_ｉは、一定の運動学的関節パラメータである。 It should be noted that the virtual measurement of the first vector in the coordinate system of the first sensor S1 arranged on the upper vehicle body L1 can be performed according to Equation 3.
(Equation 3) _S1 nmess _,1 = (R _{L1, S1} ) ^T (R _{U, L1} ) ^T e _z
Here, R _L1,S1 represents the orientation of the first sensor S1 with respect to the upper body L1 and indicates a constant application parameter that can be assumed to be known. RU _,L1 represents the rotation component of the transformation matrix _TU,L1 between the upper vehicle body L1 and the lower vehicle body U (see FIG. 2), that is, the so-called A matrix in Denavit-Hartenberg notation. In the paper "Robot modeling and control" by Spong et al. (equations 3 and 10) mentioned at the beginning, this is defined as follows.

where θ _i represents the variable joint angle and d _i , α _i and a _i are constant kinematic joint parameters.

ここで、Ｒ_Ｌ１，Ｕは、Ｒ_Ｕ，Ｌ１の逆回転行列又は転置回転行列を表す。 Therefore, for the rotational component:

Here, R _L1,U represents the inverse rotation matrix or transposed rotation matrix of R _U,L1 .

Equation 3 can be further simplified since the vector parallel to the z-axis is invariant to rotation about the z-axis, so that the orientation R of the first sensor _S1 with respect to the superstructure L1 It depends only on certain parameters of joint kinematics represented by _{L1, S1} or parameters _ai .

At the same time, the first vector _W n _ref can be specified as a reference from the point of view of the global coordinate system W according to Eq.
(Equation 7) _W n _{ref, 1} = R _{W, U} e _z

It should be noted that both the description of the first vector _S1 n _mess of virtual measurements for the first sensor S1 and the description of the first vector _W n _ref,1 as a reference from the point of view of the global coordinate system W, It should be noted that the observed link, ie the state estimate for the upper body L1 or the state estimate for the first sensor S1, is not used. Therefore, the relationship expressed by Equation 8 (corresponding to Equation 1 above for the first sensor S1) minimizes the difference vector between the virtual measured value and the expected reference value _W n _ref,1 To do so, it can be introduced in the filtering 21 for the state estimation of the orientation RW _,S1 of the first sensor S1.
(Formula 8) _S1 n _mess - (R _{W, S1} ) ^TW _{n ref, 1}

The step 101 of determining the second vector pair _S1 o _mess,1 , _W o _ref,1 (also for the first sensor S1) is as follows.

It should be noted that the virtual measurement of the second vector in the coordinate system of the first sensor S1 arranged on the upper vehicle body L1 can be performed according to Equation 9.
(Equation 9) _S1 o _mess,1 = (R _{S1, L1} ) e _x
Here, R _S1,L1 denotes the inverse or transposed rotation matrix of R _L1,S1 , thus similarly representing the orientation of the first sensor S1 with respect to the upper body L1 and assumed to be known. is a constant application parameter that allows

At the same time, a second vector _W o _ref,1 can be specified as a reference in terms of the global coordinate system W according to equation 10 via the state estimate of the orientation of the preceding link, namely the underbody U. Furthermore, the rotation component RU _,L1 of the transformation matrix TU, _L1 between the upper vehicle body L1 and the lower vehicle body U can be specified.

This vector depends only on the orientation of the underbody U with respect to the frame of reference W and the measured joint angle θ ₁ . More generally, this vector depends only on the orientation estimate of the preceding module and the joint angle between the two modules.

It should be noted that here also the description of the second vector _S1 o _mess,1 for the first sensor S1 and the description of the second vector _W o _ref,1 as a reference from the point of view of the global coordinate system W , the observed module, ie the state estimate for the upper body L1 or the state estimate for the first sensor S1, is not used. Therefore, the relationship expressed by Equation 11 (corresponding to Equation 11 below for the first sensor S1) minimizes the difference vector between the virtual measured value and the expected reference value _W o _ref,1 To do so, it can be introduced in the filtering 21 for the first sensor.
(Formula 11) _S1 o _mess - (R _{W, S1} ) ^T · _W o _{ref, 1}

Step 110 of determining the first vector pair _S2 n _mess,2 , _W n _ref,2 shown in FIG. 3 and the second vector pair _S2 o _mess,2 , _W o _ref,2 for the second sensor S2. The step of determining 111 can be performed in a similar manner. This also applies to the vector determination steps for the other sensors S3, S4, S5, but these are not shown in FIG. 3 for reasons of clarity.

Claims (9)

The positions and orientations of a plurality of modules (L1, L2, L3, L4, L5) movable relative to one another via joints (J1, J2, J3, J4, J5) of a common system are determined by said modules (L1, L2, L3, A method for state estimation using a plurality of inertial sensors (S1, S2, S3, S4, S5) arranged on L4, L5),
at least one of said joints (J1, J2, J3, J4, J5) and two modules (L1, L2, L3, L4, L5) connected to said joints (J1, J2, J3, J4, J5); at least one pair of vectors ( _{Sinmess ,i} , _{Wnref ,i} or _{Siomess ,i} , _{Woref ,i} ) representing the kinematic relationship of from the measured vector and the reference vector At least one pair of vectors ( _{Sinmess ,i} , _{Wnref ,i} or Siomess _,i , _Woref , _i ) is determined ₍ 100 , 101 , 110, 111) such that
said at least one vector pair ( _{Sinmess ,i} , _{Wnref ,i} or _{Siomess ,i, Woref,i} ) is introduced into said state estimation ;
The positions and orientations of the plurality of modules (L1, L2, L3, L4 , L5 ) are defined by the at least one pair of vectors ( _{Sin mess,i} , _{Wnref ,i} or _{Siomess ,i,} Woref _{, i} ) to estimate the state based on
A method characterized by: The first vector pair ( _{Sinmess ,i} , _{Wnref ,i} ) represents the two modules (L1, L2, L3, L4, L5) represent the kinematic relationship that they have the same joint axis from each point of view,
Each of the viewpoints indicates that a predetermined coordinate system is used for observation ,
The method of claim 1. The second vector pair ( _{Siomess ,i} , _{Woref ,i} ) is the measured joint angle (θ) along one link ( L1, L2, L3, L4, L5) at least one axis ( e _x ) , representing the kinematic relationship that presets
Assuming that the joint angles (θ) are measured and the orientation of one of the links (L1, L2, L3, L4, L5 ₎ is known, the second pair of vectors (Si o _mess , _i , _Wo ref _{, i} ) indicates the orientation of said at least one axis (e _x ) of said other link (L1, L2, L3, L4, L5);
3. A method according to claim 1 or 2. said at least one vector pair ( _{Sinmess ,i} , _{Wnref ,i} or _{Siomess ,i} , _{Woref ,i} ) assigned to said module (L1, L2, L3, L4, L5) introduced in fusion of sensor data of said inertial sensors (S1, S2, S3, S4, S5),
4. A method according to any one of claims 1-3. said fusion of sensor data is performed by filtering (21, 31);
5. The method of claim 4. A computer program arranged to implement the steps of the method according to any one of claims 1 to 5. A machine-readable storage medium storing a computer program according to claim 6. 6. An electronic controller adapted to perform position and orientation estimation of a plurality of mutually movable modules using the method of any one of claims 1-5. 6. A working machine (1) having a multi-jointed arm (2), wherein a plurality of modules (L2, L3, L4, L5) correspond to links of said arm (2). or the use of the method of paragraph 1.

Images