METHOD OF DETERMINING AN ANGLE OF A TOOL OF A MACHINE

Abstract

The present invention relates to a method of determining an angle of a piece of working equipment of a machine, wherein the machine has an undercarriage and a superstructure rotatable with respect thereto, wherein the piece of working equipment is fastened to the superstructure via a swivel joint such that the angle of rotation of the swivel joint is orthogonal to the axis of rotation of the rotatable superstructure, wherein the piece of working equipment is provided with an IMU, that is in an inertial measurement unit, that is configured to detect an angular speed in three spatial directions that are preferably perpendicular to one another, and wherein a first of the three spatial directions, whose angular speed ({dot over (θ)}.sub.y) is detectable by the IMU is in parallel with the axis of rotation of the swivel joint. The method is characterised in that an angular speed ({dot over (θ)}t.sub.z) that occurs on a rotation of the superstructure is detected by the IMU and an angle of the piece of working equipment relative to the axis of rotation of the superstructure is determined on the basis of the detected angular speed ({dot over (θ)}t.sub.z) of the superstructure.

Claims

1. A method of determining an angle of a piece of working equipment of a machine, wherein the machine has an undercarriage and a superstructure rotatable with respect thereto; the piece of working equipment is fastened to the superstructure via a swivel joint such that an axis of rotation of the swivel joint is orthogonal to the axis of rotation of a rotatable superstructure; the piece of working equipment is provided with an internal measurement unit (IMU), that is configured to detect an angular speed in three spatial directions (x, y, z); and a first one of the three spatial directions (y) whose angular speed ({dot over (θ)}.sub.y) is detectable by the IMU is in parallel with the axis of rotation of the swivel joint, wherein an angular speed ({dot over (θ)}t.sub.z) occurring on a rotation of the superstructure is detected by the IMU; and an angle of the piece of working equipment relative to the axis of rotation is determined on the basis of the detected superstructure angular speed ({dot over (θ)}t.sub.z).

2. The method in accordance with claim 1, wherein the angular speed ({dot over (θ)}t.sub.z) occurring on a rotation of the superstructure is reflected in the angular speeds ({dot over (θ)}.sub.x, {dot over (θ)}.sub.z) of two spatial directions (x, z) of the IMU that differ from the first one of the three spatial directions (y) and the angle of the piece of working equipment is determined from it.

3. The method in accordance with claim 1, wherein the two angular speeds ({dot over (θ)}.sub.x, {dot over (θ)}.sub.z) of the IMU for the spatial directions that differ from the first one of the three spatial directions are to be used as arguments for a mathematical function a tan 2 to determine the angle of the piece of working equipment.

4. The method in accordance with claim 1, wherein the angle of the piece of working equipment is determined using the formula:
α.sub.G=a tan 2(sign({dot over (θ)}t.sub.z).Math.{dot over (θ)}.sub.x;sign({dot over (θ)}t.sub.z).Math.{dot over (θ)}.sub.z) wherein α.sub.G is the angle of the piece of working equipment relative to a direction of rotation axis of the superstructure; {dot over (θ)}t.sub.z is the angular speed of a rotation of the superstructure; {dot over (θ)}.sub.x is the angular speed detected by the IMU in a second one of the three spatial directions; and {dot over (θ)}.sub.z is the angular speed detected by the IMU in a third one of the three spatial directions.

5. The method in accordance with claim 1, wherein the angle of the piece of working equipment is only determined on the basis of the angular speed ({dot over (θ)}t.sub.z) of a rotation of the superstructure when the angular speed ({dot over (θ)}t.sub.z) is above a threshold value.

6. The method in accordance with claim 5, wherein, when the angular speed ({dot over (θ)}t.sub.z) of a rotation of the superstructure is below the threshold value or at the threshold value, the angle of the piece of working equipment is determined via an alternative method.

7. A machine, comprising: an undercarriage; a superstructure rotatable with respect to the undercarriage; a piece of working equipment that is fastened to the superstructure via a swivel joint such that the axis of rotation of the swivel joint is orthogonal to the axis of rotation of the rotatable superstructure; and an internal measurement unit (IMU), that is provided at the piece of working equipment and that is configured to detect an angular speed in three spatial directions (x, y, z), wherein a first one of the three spatial directions (y) whose angular speed ({dot over (θ)}.sub.y) is detectable by the IMU is in parallel with the axis of rotation of the swivel joint, wherein an angle determination unit for determining an angle of the piece of working equipment relative to the axis of rotation of the superstructure, with the angle determination unit being configured to determine the angle of the piece of working equipment on the basis of the angular speed ({dot over (θ)}t.sub.z) detected by the IMU and occurring on a rotation of the superstructure.

8. The machine in accordance with claim 7, wherein the spatial directions detected by the IMU are orthogonal to one another.

9. The machine in accordance with claim 7, wherein the angular speed ({dot over (θ)}t.sub.z) occurring on a rotation of the superstructure is reflected in the angular speeds ({dot over (θ)}.sub.x, {dot over (θ)}.sub.z) of the two spatial directions (x, z) of the IMU that differ from the first one of the three spatial directions (y) and the angle of the piece of working equipment is determined from it.

10. The machine in accordance with claim 7, wherein the angle determination unit is configured to use the two angular speeds ({dot over (θ)}.sub.x, {dot over (θ)}.sub.z) for the spatial directions (x, z) that differ from the first one of the three spatial directions (y) as an argument for the mathematical function a tan 2 to determine the angle of the piece of working equipment.

11. The machine in accordance with claim 7, wherein the angle determination unit is configured to only determine the angle of the piece of working equipment on the basis of the angular speed ({dot over (θ)}t.sub.z) of a rotation of the superstructure when the angular speed ({dot over (θ)}t.sub.z) is above a threshold value and to determine the angle of the piece of working equipment via an alternative method.

12. The machine in accordance with claim 7, wherein the machine is an excavator and the piece of working equipment is an excavator arm that has an excavator bucket, an excavator stick, and an excavator boom, wherein the IMU is arranged at at least one of the elements of the excavator arm to determine an angle of the corresponding element of the excavator arm.

13. The machine in accordance with claim 12, wherein a respective IMU is provided at the superstructure, at the excavator bucket, at the excavator stick, and at the excavator boom, said IMUs being connected to the angle determination unit via a data line.

14. The machine in accordance with claim 12, wherein the elements of the excavator arm are connected to one another via swivel joints whose axes of rotation are in parallel with one another and thus all stand perpendicular on the axis of rotation of a superstructure rotation.

15. The machine in accordance with claim 1, wherein the angular determination unit is part of an electronic control unit that is connected to the control of the machine.

16. The method of claim 6, wherein the angle of the piece of working equipment is determined based on the angle accelerated detected by the IMU.

17. The machine of claim 10, wherein the angle of the piece of working equipment, is determined using the formula:
α.sub.G=a tan 2(sign({dot over (θ)}t.sub.z).Math.{dot over (θ)}.sub.x;sign({dot over (θ)}t.sub.z).Math.{dot over (θ)}.sub.z), where α.sub.G is the angle of the piece of working equipment relative to a direction of rotation axis of the superstructure: {dot over (θ)}t.sub.z is the angular speed of a rotation of the superstructure; {dot over (θ)}.sub.x is the angular speed detected by the IMU in a second one of the three spatial directions; and {dot over (θ)}.sub.z is the angular speed detected by the IMU in a third one of the three spatial directions.

18. The machine of claim 11, wherein the angle of the piece of working equipment via the alternative method is based on the acceleration detected by the IMU when the angular speed of the rotation of the superstructure is below the threshold value or at the threshold value.

Description

[0030] Further advantages, details, and features of the present invention will become clear with reference to the following description of the Figures. There are shown:

[0031] FIG. 1: a schematic representation of a machine with a piece of working equipment that is provided with an IMU;

[0032] FIG. 2: a side view of a mine excavator with different arrangement positions of different IMUs: and

[0033] FIG. 3: an implementation of the method in accordance with the invention in an abstract illustration.

[0034] FIG. 1 here shows a schematic representation of the invention. The machine 1 can be recognized that is shown schematically only by the superstructure 4 and a piece of working equipment 2, for example an excavator boom, fastened thereto.

[0035] An IMU 8 that can detect an angular speed in three spatial directions is fixedly installed at the excavator boom 2. These three spatial directions are orthogonal to one another, with one of the three spatial directions being in parallel with the axis of rotation 5 of the swivel joint 6 by which the piece of working equipment 2 is pivotably arranged at the superstructure 4 of the machine 1. The swivel joint 6 can here correspond to a hinged joint. If the superstructure 4 now rotates about the axis of rotation 7 shown in FIG. 1 at the speed ({dot over (θ)}t.sub.z), this results in an angular speed whose vector is oriented in parallel with the axis of rotation 7. The corresponding vector can naturally also extend in the opposite sense to the arrow of the axis of rotation 7. The IMU gyrometers, that is those elements that detect the angular speed that are arranged orthogonal to the axis of rotation 5 of the swivel joint 6 can then precisely measure the projections of the angular speed. This is primarily done at the angular speeds ({dot over (θ)}.sub.x, {dot over (θ)}.sub.z). It is therefore possible to determine the angle of a piece of working equipment or of an IMU 8 connected to the piece of working equipment in which reference is made to the axis of rotation of the superstructure 4 while using these two speeds ({dot over (θ)}.sub.x, {dot over (θ)}.sub.z). In the simplest implementation, the calculation can be formed by:

α.sub.G=a tan 2(sign(θ{dot over (t)}.sub.z).Math.{dot over (θ)}.sub.x;sign({dot over (θ)}t.sub.z).Math.{dot over (θ)}.sub.z)

[0036] where α.sub.G is the angle of the piece of working equipment relative to a direction of rotation axis of the superstructure, {dot over (θ)}t.sub.z is the angular speed of a rotation of the superstructure, {dot over (θ)}.sub.x is the angular speed detected by the IMU in a second one of the three spatial directions, and {dot over (θ)}.sub.z is the angular speed detected by the IMU in a third one of the three spatial directions.

[0037] The procedure is rather similar to a method of localizing the gravity in the accelerometers, but the same physical values are not observed here since it is the aim in the present case to find the rotational speed of the superstructure in the gyrometer measurements. This approach was not pursued in any prior art available up to the date of application.

[0038] This is advantageous since there are no parasitic speeds that 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 a piece of working equipment 2. In addition, the gyrometers are not disturbed by blows and vibrations so that a reading of the relevant data can take place more simply than a reading of accelerations and accordingly also requires less filtering.

[0039] It is accordingly possible during a work cycle of the machine using the invention to precisely determine the angles of the piece of working equipment while making use of the idea explained in the present case independently of any dynamic speeds with respect to the swivel joint 6 of the piece of working equipment 2.

[0040] To also be able to determine an angle of the piece of working equipment 2 when the superstructure 4 does not perform any rotation, it is necessary to provide a fusion algorithm for a plurality of data sources, said fusion algorithm determining an angle, on the one hand, from accelerometer measurement units (conventional kind of angle determination) and, on the other hand, from gyrometers, that is angular speed measurement units when there is a superstructure rotational speed. In addition, a gyroscopic integration of collinear gyrometers at the swivel joints or at the swivel joint 6 can be provided.

[0041] In this respect, one of a plurality of possibilities for the implementation is shown in FIG. 3, wherein the rotational speed of the superstructure is compared with a threshold value B and, when the speed is below a threshold value, the calculation of a raw angle α.sub.B is performed in a conventional manner from the acceleration measurement units and, when the speed exceeds or reaches the threshold value, the raw angle α.sub.B is calculated from the gyrometers. Provision can also be made here that the raw angle α.sub.B is smoothed with the aid of a collinear gyrometer at the swivel joint. If, for example, α(t) is the angle obtained at the time t, the angle at the time α(t+1) can be calculated by α(t)+{dot over (α)}Δ.sub.t+c(−1).sup.x, where {dot over (α)} is the relative angular speed, Δ.sub.t is the time step, c is a coefficient, and x=0 if the preceding angle is smaller than the raw angle α.sub.B, or x=1 if the opposite is the case.

[0042] A complete implementation can be found in FIG. 3 in which a switching unit 10 varies the basis for calculating the angle in dependence on the speed of a rotation of the superstructure. In the position of the switching unit 10 shown, the angle is calculated in a conventional manner on the basis of the acceleration. Alternatively to this, it is possible to calculate the angle with the aid of the speeds, with this only being done when the superstructure rotation has reached a certain speed. θ{dot over (p)}.sub.y here represents the angular speed measured by the IMU that leads in a kinematic chain.

[0043] A complete system that shows the advantages of the method in accordance with the invention can be assembled as shown in FIG. 2. An IMU is provided both at the excavator bucket 11 and at the excavator stick 12 and at the excavator boom 13. A further IMU 8 is also provided at the superstructure 4. These four IMUs transmit their raw data (accelerations and angular speeds) to an electronic unit (ECU) that contains the algorithms for calculating the relative angles between every part of the excavator arm or of the superstructure. This means a calculation of the excavator bucket angle, of the excavator stick angle, of the excavator boom angle, and a superstructure angle of tilt and a superstructure roll angle. The electronic control unit ECU can also calculate all the relative speeds linked to these angles. Provision can furthermore be made that this control unit is in communication connection with excavator electronics that provide all the data via a suitable bus. This is shown by a control box (steering) in the drawing.