阅读说明：本技术 用于确定机器的作业设备的角度的方法 (Method for determining an angle of a working device of a machine ) 是由 A·杰吉 于 2019-07-26 设计创作，主要内容包括：本发明涉及一种弄用于确定机器的作业设备的角度的方法,所述机器具有底盘和可以相对于所述底盘旋转的上部结构,所述作业设备通过旋转接点固定到所述上部结构,使得所述旋转接点的旋转轴线正交于可旋转的上部结构的旋转轴线,所述作业设备具有IMU,即惯性测量单元,所述惯性测量单元设计成用于感测优选地彼此垂直的三个空间方向(x、y、z)上的角速度,并且通过IMU可感测平行于旋转接点的旋转轴线的三个空间方向中的第一个空间方向(y)的角速度(I)。所述方法的特征在于,通过IMU感测在所述上部结构旋转时发生的角速度(II),并且基于感测到的上部结构角速度(II)确定作业设备相对于上部结构的旋转轴线的角度。(The invention relates to a method for determining the angle of a working device of a machine having a chassis and a superstructure which is rotatable relative to the chassis, the working device being fixed to the superstructure by means of a rotary joint such that the axis of rotation of the rotary joint is orthogonal to the axis of rotation of the rotatable superstructure, the working device having an IMU, i.e. an inertial measurement unit, which is designed for sensing angular velocities in three spatial directions (x, y, z), preferably perpendicular to each other, and by means of which an angular velocity (I) in a first of the three spatial directions (y) parallel to the axis of rotation of the rotary joint is sensible. The method is characterised by sensing by the IMU an angular velocity (II) occurring when the superstructure is rotating, and determining the angle of the work equipment relative to the rotational axis of the superstructure based on the sensed superstructure angular velocity (II).)

1. A method for determining an angle of a working device (2) of a machine (1), wherein,

the machine (1) has a chassis (3) and a superstructure (4) rotatable relative to the chassis,

the work device (2) being fixed to the superstructure (4) by a rotary joint (6) such that the axis of rotation (5) of the rotary joint (6) is orthogonal to the axis of rotation (7) of the rotatable superstructure (4),

the working device (2) has an IMU (8), an inertial measurement unit, designed for sensing angular velocities in three spatial directions (x, V, z), preferably perpendicular to each other, and

a first of the three spatial directions (y) is parallel to the rotational axis (5) of the rotary joint (6), the angular velocity of the first of the three spatial directions (y) being sensible by the IMU (8)

It is characterized in that the preparation method is characterized in that,

sensing, by the IMU (8), angular velocities occurring as the superstructure (4) rotatesAnd is

Based on sensed angular velocity of superstructure-determining an angle of the work equipment (2) relative to a rotation axis (7) of the superstructure (4).

2. Method according to claim 1, wherein the angular velocity occurring while the superstructure (4) is rotatingReflecting angular velocities of the IMU (8) in two spatial directions (x, z) different from a first of the three spatial directions (y)And thereby determining the angle of the work apparatus (2).

3. The method according to any of the preceding claims, wherein the IMU (8) has two angular velocities in a different spatial direction than the first of the three spatial directions (y) Is used as an argument of the mathematical function atan2 to determine the angle of the work equipment (2).

4. The method according to any of the preceding claims, wherein the angle of the work equipment (2) is determined using the following formula:

wherein:

α_Gis the angle of the working device (2) relative to the direction of rotation axis of the superstructure (4),

is the angular velocity at which the superstructure (4) rotates,

an angular velocity in a second of the three spatial directions sensed for the IMU (8), and

an angular velocity in a third of the three spatial directions sensed for the IMU (8).

5. According to the frontThe method of any of the preceding claims, wherein the angular velocity is measured asAbove a threshold value (B), the angle of the working device (2) is based only on the angular speed at which the superstructure (4) rotatesTo be determined.

6. Method according to claim 5, wherein the angular velocity when the superstructure (4) is rotatedLess than or equal to the threshold value (B), the angle of the work machine (2) is determined by an alternative method, preferably based on the angular acceleration sensed by the IMU (8).

7. A machine (1), in particular an excavator, comprising:

a chassis (3) is arranged on the base,

a superstructure (4) rotatable relative to the chassis (3),

a working device (2) which is fixed to the superstructure (4) by means of a rotary joint (6) in such a way that the axis of rotation (5) of the rotary joint (6) is orthogonal to the axis of rotation (5) of the rotatable superstructure (4), and

an IMU (8), an inertial measurement unit, disposed on the work equipment (2) and designed for sensing angular velocities in three spatial directions (x, V, z), wherein

A first of the three spatial directions (y) is parallel to the rotational axis (5) of the rotary joint (6), an angular velocity of the first of the three spatial directions (y) being sensible by the IMU (8)

It is characterized in that the preparation method is characterized in that,

an angle determination unit for determining an angle of the working device (2) with respect to a rotational axis (7) of the superstructure (4), wherein

The angle determination unit is designed such that the angle of the working device (2) is based on the angular velocity sensed by the IMU (8) occurring when the superstructure (4) is rotatedTo be determined.

8. The machine (1) of claim 7, wherein the spatial directions sensed by the IMUs (8) are orthogonal to each other.

9. Machine (1) according to claim 7 or 8, wherein the angular speed that occurs when the superstructure (4) rotates isReflecting angular velocities of two spatial directions (x, z) of the IMU (8) different from the first of the three spatial directions (y)And the angle determination unit is designed to determine therefrom the angle of the working device (2).

10. Machine (1) according to any one of claims 7 to 9, wherein said angle determination unit is designed so that two angular velocities are present in a spatial direction (x, z) different from said first one (y) of said three spatial directionsUsed as an argument of the mathematical function atan2 to determine the angle of the work equipment (2), preferably using the following formula:

wherein

α_GIs the angle of the working device (2) relative to the direction of rotation axis of the superstructure (4),

is the angular velocity at which the superstructure (4) rotates,

an angular velocity in a second of the three spatial directions sensed for the IMU (8), and

an angular velocity in a third of the three spatial directions sensed for the IMU (8).

11. Machine (1) according to any one of claims 7 to 10, wherein said angle determination unit is designed such that when angular speed is prevailingAbove a threshold value (B), the angle of the working device (2) is based only on the angular speed at which the superstructure (4) rotatesAnd the angular velocity at which the superstructure (4) rotates is determinedLess than or equal to the threshold value (B), the angle of the work machine (2) is determined by an alternative method, preferably based on the acceleration sensed by the IMU (8).

12. The machine (1) according to any of claims 7 to 11, wherein the machine (1) is an excavator (9) and the work equipment (2) is an excavator arm (10) having an excavator bucket (11), an excavator stick (12) and an excavator boom (13), wherein the IMU (8) is arranged on at least one of the components of the excavator arm (10) to determine the angle of the respective component of the excavator arm (10).

13. Machine (1) according to claim 12, wherein an IMU (8) is provided on the superstructure (4), the excavator bucket (11), the excavator stick (12) and the excavator boom (13), respectively, said IMU being connected to the angle determination unit, preferably by data lines.

14. Machine (1) according to claim 12 or 13, wherein the components of the excavator arm (10) are connected to each other by means of rotary joints, the axes of rotation of which are parallel to each other and thus each perpendicular to the axis of rotation (7) of the superstructure rotation.

15. Machine (1) according to any of the claims from 7 to 14, wherein said angle determination unit is part of an electronic control device, which is kept connected with the control means of the machine (1).

Technical Field

The present invention relates to a method for determining an angle of a working device of a machine and to a corresponding machine. It is advantageous for work machines, in particular for excavators, that they can accurately determine the angle of a work implement, for example the angle of an excavator arm member. Various ways of determining such an angle are known in the art, but none of them are stable, cheap, accurate and easy to integrate. In such a case, it is desirable that the work equipment can also be easily retrofitted when the angle determination is performed.

Background

Angle calculations by means of Inertial units (IMU, english: Inertial Measurement Unit) do not satisfy all the above conditions, because their accuracy decreases at high speed. In addition, work machines, particularly excavators, are particularly affected and cannot be used simply, for example, as an inertial navigation system is used in an aircraft or vehicle.

However, an IMU (inertial measurement unit) is typically used, which is then used to measure acceleration along three axes. Gravity describes the vertical acceleration and therefore can be measured using the IMU. Observing the position of the projection of the gravity on the three axial directions of the acceleration measuring instrument can obtain the angle of the IMU relative to the vertical direction. Such a process is described for example in US 9618338B 2. Therefore, by mounting the IMU on the working device, the angle of the working device can be determined approximately accurately.

The problem is that once a motion occurs, such as a rotation of the superstructure of an excavator or other construction machine, a centrifugal force is added to the acceleration measured by the IMU. Therefore, the calculation of the angle is disadvantageous, since the projected force is no longer only vertical, but a combination of gravity and centrifugal force. In order to compensate for these effects, it is known from the prior art to utilize a geometric model of the machine or excavator in order to estimate the centrifugal forces on the sensors in real time and take them into account when calculating the orientation of the vertical gravitational force. Such a procedure is disclosed, for example, in AR 104232 a1, where relatively reliable results are given, but with the prerequisite that a geometric model of the machine or excavator with all lengths, distances and positions of multiple IMUs must be created and simulated. In addition, this solution requires a more or less complex calibration phase.

Furthermore, it is also known from the prior art to Filter out dynamic influences (centrifugal forces, etc.), for example using Kalman filters (Kalman filters) or a combination of high-pass and low-pass filters. This allows dynamic effects due to the movement of the work equipment or the vehicle to be reduced. However, this implementation does not produce the exact solution needed in all cases. Furthermore, the design of all available methods on the market requires that they need to be initially calibrated, which takes up additional resources.

In general, the existing methods aim to attenuate the adverse effects of centrifugal forces, in particular when the superstructure is rotating, to enhance the accuracy of the measurement principle used.

Disclosure of Invention

It is an object of the present invention to provide a method of determining the angle of a work apparatus which overcomes the above disadvantages. This object is achieved by a method having all the features of claim 1 or by a machine having all the features of claim 9.

Advantageous embodiments are given in the dependent claims.

The present invention relates to a method for determining the angle of a working device of a machine, in particular of an excavator, wherein the machine has a chassis and a superstructure which is rotatable relative to the chassis, the working device being fixed to the superstructure by means of a rotary joint such that the axis of rotation of the rotary joint is orthogonal to the axis of rotation of the rotatable superstructure, the working device being provided with an IMU, i.e. an inertial measurement unit, which is designed for sensing angular velocities in three spatial directions, preferably perpendicular to each other, wherein the angular velocity in a first of the three spatial directions is sensed by the IMU, which first of the three spatial directions is parallel to the axis of rotation of the rotary joint. The method is characterised by sensing by the IMU an angular velocity occurring while the superstructure is rotating and determining the angle of the work equipment relative to the rotational axis of the superstructure based on the sensed angular velocity of the superstructure.

Unlike known methods for determining the angle of a working device, the present invention does not attempt to suppress the influence of the rotation of the superstructure, but determines the angle using its characteristics.

Further research has shown that from the point of view of the machine or rather the excavator, the rotation of the superstructure is at an angular velocity that is constantly upwards (or downwards). The angular velocity or the vector of the angular velocity is always perpendicular to the plane defined by the rotation of the superstructure. The gyrometers of the IMU (i.e., the means for sensing the angular velocity of the respective spatial directions) are arranged such that two of them are orthogonal to the axis of rotation of the rotary joint of the working equipment. Finally, a first of the three spatial directions, whose angular velocity can be sensed by the IMU, is parallel to the rotational axis of the rotary joint, so that the other two spatial directions must be orthogonal to the first spatial direction. With two spatial directions arranged perpendicular to the rotation axis, these two spatial directions angular velocities can be sensed by means of the IMU, so that the projection of the angular velocity caused by the rotation of the superstructure can be measured accurately. Thus, the measured angular velocity may be used to accurately determine the angle of the IMU mounted work apparatus relative to the rotational axis of the superstructure.

According to an optional development of the invention, the angular velocity occurring when the superstructure rotates is reflected on the angular velocities of two spatial directions of the IMU different from a first of the three spatial directions, and the angle of the working equipment can be determined therefrom.

Preferably, two angular velocities of the IMU for a different spatial direction than the first of the three spatial directions are used as arguments of the mathematical function atan2 in order to determine the angle of the work machine. The function atan2, also known as arctan2, is an extension of the arctangent of an inverse trigonometric function and takes two real numbers as arguments, so that it has enough information to be able to output a function value in a range of 360 degrees, as opposed to the normal arctangent. Therefore, the present invention must also include: the angular velocity is optionally subjected to an inverse tangent function to determine the angle.

According to a preferred embodiment, the angle of the work device is determined using the following equation:

wherein: alpha is alpha_GIs the angle of the work apparatus relative to the axis of the direction of rotation of the superstructure,is the angular velocity at which the superstructure rotates,is the angular velocity in the second of the three spatial directions sensed by the IMU, andis the angular velocity in the third of the three spatial directions sensed by the IMU.

In addition to this, according to a further development of the invention it may be provided that the angle of the work apparatus is determined solely on the basis of the angular velocity of the rotation of the superstructure when the angular velocity of the rotation of the superstructure is greater than a threshold value.

It may additionally be provided that the angle of the work implement is determined by an alternative method, preferably based on acceleration sensed by the IMU, when the angular velocity of rotation of the upper structure is less than or equal to a threshold value.

Furthermore, a weighting for determining the angle of the work apparatus may be set, and the determination based on the angular velocity has a weighting factor that varies with the rotational angular velocity of the superstructure. For example, when the angular velocity of the rotation of the upper structure is high, the weighting factor may take a large value, and when the angular velocity of the rotation of the upper structure is low, the weighting factor may take a low value.

The invention also includes a machine having: a chassis; a superstructure rotatable relative to the chassis; a work implement secured to the superstructure by a rotary joint such that an axis of rotation of the rotary joint is orthogonal to an axis of rotation of the rotatable superstructure; and an IMU, an inertial measurement unit, disposed in the work apparatus and designed to sense angular velocities in three spatial directions, wherein a first of the three spatial directions is parallel to the rotational axis of the rotary joint, the angular velocity in the first of the three spatial directions being sensed by the IMU. The machine is characterized in that an angle determination unit for determining the angle of the working device relative to the rotation axis of the superstructure is provided, wherein the angle determination unit is designed such that the angle of the working device is determined on the basis of the angular velocity sensed by the market IMU occurring when the superstructure is rotated.

Similarly to the subject matter of claim 1, again such means are used that the vector of the angular velocity is orthogonal to the plane defined by the rotation, so that an angular velocity sensor sensing a spatial direction parallel to the rotation axis of the rotary joint does not register any deflection when the superstructure rotates. This is because the axis of rotation and the spatial direction parallel to the axis of rotation are arranged perpendicular to the axis of rotation of the rotatable superstructure. Thus, the angular velocity due to the rotation of the superstructure is mapped into two further angular velocity sensors in other spatial directions, so that the position of the IMU and the work equipment firmly connected thereto can be derived.

It is preferably clearly shown that the spatial directions sensed by the IMU are orthogonal to each other.

According to an advantageous embodiment of the invention, the angular velocity occurring when the superstructure is rotated is reflected on the angular velocities of two spatial directions of the IMU different from a first of the three spatial directions, and the angle determination unit is designed to determine the angle of the working device.

Preferably, the angle determination unit of the machine is designed such that two angular velocities of a different spatial direction than the first of the three spatial directions are used as arguments of the mathematical function atan2 to determine the angle of the working device. According to a preferred implementation, this is achieved by:

wherein: alpha is alpha_GIs the angle of the work apparatus relative to the axis of the direction of rotation of the superstructure,is the angular velocity at which the superstructure rotates,is the angular velocity in the second of the three spatial directions sensed by the IMU, andis the angular velocity in the third of the three spatial directions sensed by the IMU.

Furthermore, it may be provided that the angle determination unit is designed such that, when the angular velocity is greater than a threshold value, the angle of the work apparatus is determined based only on the angular velocity of the superstructure rotation, and when the angular velocity of the superstructure rotation is less than or equal to the threshold value, the angle of the work apparatus is determined by an alternative method, preferably based on an acceleration sensed by the IMU.

According to an optional refinement of the invention, the machine is an excavator and the working device is an excavator arm having an excavator bucket, an excavator stick and an excavator boom, wherein the IMU is arranged on at least one component of the excavator arm in order to determine the angle of the respective component of the excavator arm.

Furthermore, provision can be made for an IMU to be provided on the superstructure, the excavator bucket, the excavator stick and the excavator boom, respectively, which IMU is preferably connected to the angle determination unit via a data line. The angle determination unit may be designed to calculate or derive the angle of each element with its respective IMU.

According to an advantageous development of the invention, the components of the excavator arm are connected to each other by means of swivel joints, the swivel axes of which are parallel to each other and thus are all perpendicular to the swivel axis of rotation of the superstructure.

Furthermore, it may be provided that the angle-determining unit is part of an electronic control device which is connected to a control device of the machine.

Drawings

Other advantages, details and technical features of the present invention may be obtained from the following description of the drawings. Wherein:

FIG. 1 shows a schematic view of a machine having a work apparatus provided with an IMU;

fig. 2 shows a side view of a mining excavator with various IMUs in different deployment positions;

fig. 3 shows an abstract schematic diagram for implementing the method according to the invention.

Detailed Description

Fig. 1 shows a schematic diagram of the present invention. A machine 1 can be seen, which only schematically shows an upper structure 4 and a work device 2, such as an excavator boom, fixed thereto.

The IMU 8 is fixedly mounted on the excavator boom 2, and the IMU 8 can sense angular velocity in three spatial directions. The three spatial directions are orthogonal to one another, one of which is parallel to the axis of rotation 5 of the rotary joint 6, by means of which the working device 2 is operatedThe rotary joint is pivotably arranged on the upper structure 4 of the machine 1. The pivot joint 6 can correspond to a hinge joint (Scharniergelenk). If the superstructure 4 is now at speedRotating around the axis of rotation 7 shown in fig. 1, this then results in an angular velocity, the vector of which is oriented parallel to the axis of rotation 7. The corresponding vector can of course also extend in the opposite direction to the arrow of the axis of rotation 7. The element sensing angular velocity, i.e. the IMU gyrometer arranged orthogonally to the rotation axis 5 of the rotary joint 6, can accurately measure the projection of angular velocity. This is mainly by angular velocityAnd (5) realizing. Thus, it is possible to use these two speedsThe angle of the working equipment or the IMU 8 firmly connected to the working equipment is determined with reference to the axis of rotation of the superstructure 4. In the simplest embodiment, this can be calculated by:

wherein: alpha is alpha_GIs the angle of the work apparatus relative to the axis of the direction of rotation of the superstructure,is the angular velocity at which the superstructure rotates,is the angular velocity in the second of the three spatial directions sensed by the IMU, anIs the angular velocity in the third of the three spatial directions sensed by the IMU.

This procedure is very similar to the method of finding the gravity in acceleration measurements, but the same physical values are not observed here, since here the aim is to find the rotation speed of the superstructure in gyrometer measurements. None of the prior art until the filing date follows this approach.

This is advantageous because there are no parasitic velocities: (Geschwidtigkeiten) can interfere with the calculations. This is due to the fact that the rotational speed of the superstructure is perpendicular to the swivel joint 6 of the work machine 2. Furthermore, the gyrometer is not disturbed by jolts and vibrations, so the relevant data can be read more easily than the acceleration, and therefore less filtering is required.

The present invention can accurately determine the angle of the work implement during the work cycle of the machine using the concepts described herein, regardless of any dynamic speed of the rotating joint 6 relative to the work implement 2.

In order to be able to determine the angle of the work apparatus 2 also when the upper structure 4 is not rotating, it is necessary to provide a fusion algorithm (fusion salgorithmus) for the plurality of data sources, which algorithm uses an acceleration measuring unit (a conventional type of angle determination) on the one hand and a gyro-tester (i.e. an angular velocity measuring unit) on the other hand when there is a rotational speed of the upper structure. Alternatively, a gyrometer integration of collinear gyrometers may be provided at multiple or one of the rotary joints 6.

One of the various implementation possibilities is shown in fig. 3, in which the rotational speed of the superstructure is compared with a threshold value B, and if the speed is below the threshold value, the raw angle α is calculated from the acceleration measuring unit in a conventional manner and method_BIf the speed exceeds or reaches a threshold, then the raw angle α is calculated from the gyrometer_B. It can also be provided that the original angle alpha is brought to the rotary joint by means of a collinear gyrometer_BAnd (6) smoothing. For example, if α (t) is an angle obtained at time t, α (t) + α may be used^·Δ_t+c(-1)^xTo calculate the angle of a (t +1) at a point in time, where a^·Is the relative angular velocity, Δ_tIs the time step (Zeitschritt), c is the coefficient, if the previous angle is smaller than the original angle alpha_BIf x is 0, otherwise, x is 1.

A complete embodiment can be found in fig. 3, where the switching unit 10 changes the basis for calculating the angle depending on the rotational speed of the superstructure. In the shown position of the switching unit 10, the angle is calculated in a conventional manner and method on the basis of the acceleration. Alternatively, the angle may be calculated by means of speed, which is only done when the superstructure rotates up to a certain speed.Indicates the angular velocity measured by the IMU, which precedes in the kinematic chain (kinematischen key).

The complete system with the advantages of the method according to the invention can be summarized as shown in fig. 2. The excavator bucket 11, the excavator stick 12, and the excavator boom 13 are provided with IMUs. Another IMU 8 is also provided on the superstructure 4. The four IMUs send their raw data (acceleration and angular velocity) to an electronic unit (ECU) containing algorithms for calculating the relative angles between the parts of the excavator arm or superstructure. This means that the excavator bucket angle, excavator stick angle, excavator boom angle and superstructure roll angle need to be calculated. The electronic control unit ECU can also calculate all the relative speeds associated with these angles. It can also be provided that the control unit establishes a communication link with the excavator electronics, which provides all data via a suitable bus. This is represented in the figure by the control box (steering).