Patent Application
20230417025
The present disclosure relates to a system for determining a swing boom angle of an excavator, wherein the excavator comprises a lower part, an upper part comprising a cabin, the upper part being arranged on the lower part and configured to be rotated relative to the lower part about a first rotation axis and a swing boom arranged on the upper part and configured to be rotated relative to the upper part about a second rotation axis that an actual swing boom position defines a swing boom angle.
Large scale excavators are earth moving machines that consist of a lower part that rests on the ground and supports tracks or wheels to move the excavator, a rotating upper part mounted to the lower part by means of a large bearing with a first vertical rotation axis and a dig structure that is composed of several components attached to each other by means of linkage pins with horizontal rotation axis that are parallel to each other.
Miniature excavators are also capable of swinging the dig structure relative to the rotating upper part. There is a pin with a second vertical rotation axis in between the dig structure and the rotating upper part. This type of construction is called a swing boom because the boom swings relative to the upper part resulting in a swing boom angle.
Any grade control system designed for a fixed boom excavator requires additional sensors or software in order to correctly track the motion (yaw rotation) of the boom relative to the upper part on a swing boom excavator as the system cannot use gravity to determine the position. Further, they are able to measure changes in position, but they require an initialization or calibration scheme to determine an absolute position and in order to periodically reject accumulated measurement errors.
A typical excavator grade control system utilizes IMUs (inertial measurement units) to measure the motion of the swing boom relative to the rotating upper part. Accelerometers identify the direction of gravity at rest, and during periods of motion gyroscopes are integrated to track the displacement of the linkage. Often the accelerometer and gyroscope signals are combined using some form of filtering. Many methods are possible and can be conceptually reduced to applying a low pass filter to the accelerometer and high pass filter to the gyroscopes. Over time the integration of the gyroscope measurement will drift due to uncorrected bias and sensor noise. Gravity cannot be used to stabilize the swing boom rotation as the rotational axis is aligned or nearly aligned with gravity.
DK180402B1 discloses a wire sensor to measure the length of the cylinder used to actuate a swing boom movement on mini excavators with swing boom functionality. The wire sensor is a self-retracting spool of wire that has an angular encoder mounted to measure rotation of the spool relative to the housing. In this installation, the housing is mounted onto the main upper part of the excavator. The wire is extended and affixed to the swing boom structure. There is a one-to-one correspondence between the measurement of the angular encoder and the swing boom angle with respect to the upper part.
There is a need to develop systems which determine accurately the swing boom angle of excavator without having a long-term drift of the IMU-based measurement. Further, systems are required which provide alternative approaches to track the swing boom angle and do not rely on a wire sensor.
It is therefore an object of the disclosure to provide an improved system for determining a swing boom angle of an excavator.
This object is achieved by realizing at least part of the features of the independent claims. Features which further develop the disclosure in an alternative or advantageous manner are described in the dependent patent claims.
The disclosure relates to a system for determining a swing boom angle of an excavator, wherein the excavator comprises a lower part, an upper part comprising a cabin, the upper part being arranged on the lower part and configured to be rotated relative to the lower part about a first rotation axis and a swing boom arranged on the upper part and configured to be rotated relative to the upper part about a second rotation axis that an actual swing boom position defines a swing boom angle. The system comprises a first inertial measurement unit (IMU) configured to be mounted on the swing boom and to generate first IMU data, wherein the first IMU comprises at least one acceleration sensor (accelerometer) and a processing unit. The processing unit is configured to receive the first IMU data, determine a direction of a centripetal acceleration acting on the first IMU based on the received first IMU data when the upper part rotates about the first rotation axis relative to the lower part and determine the swing boom angle based on the direction of the centripetal acceleration.
To determine when the upper part rotates, the processing unit receives information regarding the rotational motion state (i.e. whether the upper part is currently rotating) and reads the first IMU data coupled with this rotational motion state information. Examples of how rotational motion state information can be provided are: Can-bus of the excavator, reading of the excavator's control data, reading of the angle encoder on the first rotation axis, GPS on the cabin, etc.
It is clear to the skilled person that the number and type of the at least one acceleration sensor (accelerometer) has to be selected depending on the respective measuring task, embodiment of the disclosure, etc., in order to ensure the feasibility and the required measuring accuracy (e.g. if a single axis accelerometer is not appropriate for the specific measuring task, the skilled person would use e.g. multiple single axis accelerometers or e.g. one tri-axial accelerometer). By way of example and in a non-restrictive manner, at least a pair of accelerometers may be necessary to define the direction of the centripetal acceleration in the plane defined by the directions of the two accelerometer sense elements.
The rotation of the swing boom can be determined/identified, for example, by using one gyroscope mounted on the upper part and a another gyroscope mounted on the swing boom. A non-zero angular velocity measured by the gyroscope mounted on the upper part indicates the swing/rotation movement of the upper part. A difference in measured angular velocity between the gyroscope mounted on the upper part and the gyroscope mounted on the swing boom indicates swing boom motion relative to the upper part.
Further examples of how rotational motion (rotation of the upper part) state information can be provided are:
The first and the second rotation axis are vertical axis which are parallel to each other. The dig structure is composed of several components including the swing boom attached to each other by means of linkage pins with horizontal rotation axis that are parallel to each other but perpendicular to the first and the second rotation axis. The swing boom rotates about such a horizontal axis when it is being raised or lowered.
When an IMU is mounted on a moving component (e.g. the swing boom), it measures the accelerations applied to the sense element. These accelerations can be decomposed into a gravitational component, a linear component related to transport motion of the lower part and components related to rigid body motion of the excavator components relative to each other (including centripetal accelerations caused by the rotation of the upper part about the first rotation axis and the rotation of the swing boom about the second rotation axis).
In addition to the at least one accelerometer, the disclosed IMUs may be composed of further components. The IMUs can comprise further accelerometers, in particular two accelerometers. Furthermore, the IMUs can also comprise additionally to the at least one accelerometer at least one gyroscope. In this manner, the IMUs can comprise two accelerometers and one gyroscope. An IMU containing two accelerometers makes sense especially if both accelerometers are single-axis accelerometers. When using three-axis accelerometers, it is also possible to use an IMU with only one accelerometer.
In a further embodiment, the system further comprises a display unit configured to be arranged in the cabin, wherein the display unit is connected to the processing unit. The display unit is configured to provide a visualization of swing boom angle information to an operator based on the swing boom angle.
The visualization of the swing boom angle information can be realised in different ways. On the one hand, the specific value of the swing boom angle can be shown as a number on the display. On the other hand, it is also possible to display the swing boom angle by colour coding or on a colour scale. For example, a red colour can be displayed for a swing boom position left (negative angle values) of the middle position (zero degrees), which changes to a blue colour for a swing boom position right (positive angle values) of the middle position. The gradation of the individual angle values can be done by mixing different colours. Also the display via a bar, which increases in length as the value of the swing boom angle increases, is conceivable. It is also possible to display the swing boom angle via a scale on which a needle moves according to the movement of the swing boom (comparable to a compass) and thus displays the corresponding swing boom angle. Other embodiments for visualization of the swing boom angle information are also conceivable.
In a further embodiment, the first IMU determines an angular velocity of the rotation of the swing boom about the first rotation axis, the first IMU sends out a signal/information about the angular velocity of the rotation of the swing boom about the first rotation axis and the processing unit is configured to receive the signal/information about the angular velocity of the rotation of the swing boom about the first rotation axis and to determine if the swing boom is rotating at a minimum angular velocity, wherein above this minimum angular velocity a determination of the centripetal acceleration direction is possible.
In other words, this minimum threshold (minimum angular velocity) for the swing/rotation speed of the swing boom must be exceeded in order to provide sufficient observability for the centripetal acceleration direction to be usable. Further, the system can compensate for the long-term drift of the IMU measurement as long as the excavator operation includes sufficient swinging of the upper part relative to the lower part.
The value for the minimum threshold can be determined based on a sensitivity analysis. The threshold can be estimated if the desired accuracy, the distance of the sensor (first IMU) from the center of rotation (first rotation axis), and the sensor measurement noise values are given. In practice a threshold of 2-3 rpm achieves a reasonable performance.
Since the centripetal acceleration is always perpendicular to the respective rotation axis and points to this rotation axis (center of rotation), the centripetal acceleration angle changes with regard to the mounting position of the first IMU on the swing boom. For this reason, the determination of the swing boom angle by means of the disclosed system depends on the mounting position of the first IMU, but the position of the first IMU does not necessarily have to be known, as the position can be determined by means of a suitable calibration.
In a further embodiment, the system further comprises a second IMU, wherein the second IMU comprises at least one acceleration sensor, configured to be mounted on the upper part. With the second IMU, comprising an accelerometer, mounted on the upper part, the gravitational components of the measured accelerations can be removed when the IMUs are aligned and the upper part rotates.
In a further embodiment, the first IMU comprises a first gyroscope and the second IMU comprises a second gyroscope, wherein the first gyroscope is configured to provide data about the position of the swing boom as first position data and the second gyroscope is configured to provide data about the position of the upper part as second position data.
In a further embodiment, the processing unit is further configured to receive the first and second position data, combine the first and second position data with the first and the second IMU data and determine the swing boom angle based on the data combination.
In other words, the upper part rotation rate is subtracted from the measured swing boom rotation rate to isolate relative rotation rate for integration. This also enables the use of the gyroscopes to calculate centripetal acceleration if desired.
In a further embodiment, the combination of the first and second position data with the first and the second IMU data is carried out by a sensor fusion algorithm.
In a further embodiment, the sensor fusion algorithm is a complementary filter and/or a Kalman filter and/or an iterative root finding scheme (iterative root seeking algorithm) and/or any other suitable method for combining the swing boom angle measurements.
The disclosed measurement system could be combined with gyroscope (gyro) integration to track the rotation of the upper part and the swing boom. During periods when the rotating upper part is stationary or rotating at low speed, the angular velocity of the upper part and the swing boom could be compared and a relative angular velocity could be calculated in the swing boom direction (subtracting the rotation rate of the upper part from the measured swing boom rotation rate to isolate the relative rotation rate). This relative angular velocity can be integrated to track the change in the swing boom angle. Whenever the upper part rotates relative to the lower part, the disclosed determination of the centripetal acceleration direction can be combined with the integrated swing boom angle measurement (usage of the first and second gyroscopes) using a sensor fusion algorithm. This would combine the two measurements, combining the continuous tracking provided by gyroscope integration with the disclosed system of directly measuring the swing boom angle when the upper part is rotating relative to the lower part.
In a further embodiment, the swing boom further comprises an arm and a bucket being attached to the swing boom. The first IMU can be mounted on the swing boom, the arm or the bucket of the excavator. The position of the first IMU does not need to be known exactly, as it can be determined by calibration. If the first IMU is mounted on the arm or the bucket, the first IMU is located further from the second rotation axis and therefor will provide an observable measurement at a lower rotation/swing speed.
In a further embodiment, the first IMU data are transformed from a coordinate system associated with the first IMU into a coordinate system associated with the upper part prior to the determination of the swing boom angle by the processing unit.
In a further embodiment, the system further comprises a motion model configured to track a motion of the upper part, the lower part or the swing boom, determine a type of motion based on the tracked motion, determine a motion acceleration and a gravitational acceleration based on the tracked motion of upper part, lower part or swing boom and change an execution of the sensor fusion algorithm and/or the determination of the direction of the centripetal acceleration based on the determined type of motion.
The motion is tracked to consist of transient behaviors with many contributions (this could be vibration or Euler accelerations or disturbances due to digging, etc.) and in response to this tracking the parameters of the sensor fusion algorithm are changed to prevent the sensor fusion from entraining errors into the fused estimate for the swing boom angle. Further, cases can be identified where the centripetal acceleration cannot be identified and therefore greater reliance is placed on gyroscope integration techniques to track swing boom angle during these periods.
In a further embodiment, the centripetal acceleration of the tracked motion is calculated by subtracting the gravitational acceleration from the motion acceleration.
In a further embodiment, the motion is tracked as a rotation of the upper part relative to the lower part about the vertical first rotation axis and a rotation of the swing boom relative to the upper part about a horizontal third rotation axis, wherein the third rotation axis is perpendicular to the first rotation axis. Based on the tracked motion, a centripetal acceleration of the rotation motion of the swing boom about the third rotation axis is determined based on the position of the first IMU and a centripetal acceleration of the rotation motion of the upper part about the first rotation axis is calculated by subtracting the gravitational acceleration and the centripetal acceleration of the rotation motion of the swing boom about the third rotation axis from the motion acceleration of the upper part.
In other words, in a specific embodiment, the motion is determined to consist of cabin swing motion in addition to lift/lower motion of the swing boom and in response to this determination:
The position of the first IMU on the swing boom is known, so the exact direction of the centripetal motion experienced due to lift or lower motion of the boom is also known. So if the exact direction of the centripetal motion and the gravity are subtracted from the measured acceleration the cabin swing centripetal acceleration can be isolated.
In a further embodiment, the motion is tracked as a rotation of the upper part relative to the lower part about the first rotation axis and a rotation of the swing boom relative to the upper part about the second rotation axis. Based on the tracked motion, a centripetal acceleration of the rotation motion of the swing boom about the second rotation axis is determined based on the position of the first IMU and a centripetal acceleration of the rotation motion of the upper part about the first rotation axis is calculated by subtracting the gravitational acceleration and the determined centripetal acceleration of the rotation motion of the swing boom about the second rotation axis from the motion acceleration of the upper part.
In other words, in a specific embodiment, the motion is determined to consist of cabin swing motion in addition to boom swing motion relative to the cabin and in response to this determination
Because the position of the first IMU on the swing boom is known, the direction of that centripetal component in IMU coordinates is also known. The gyroscopes and position of the first IMU on the swing boom can be used to calculate the centripetal acceleration due to swing boom motion, and then isolate the cabin swing centripetal acceleration.
In a further embodiment, the system is configured to self-calibrate based on a self-calibration procedure including the steps:
In a further embodiment, the self-calibration procedure in particular further includes the steps:
The disclosure is illustrated in more detail below, purely by way of example, with reference to working examples shown schematically in the drawing. Identical elements are labelled with the same reference numerals in the figures. The described embodiments are generally not shown true to scale and they are also not to be interpreted as limiting.
A
C=ω×ω×ρ
The centripetal force FZ resulting from the centripetal acceleration Acentripetal always points from the measurement position towards the center of rotation. This means that if the centripetal force FZ induced by the rotation of the upper part about the first rotation axis 7 can be isolated, the direction of the centripetal force FZ in the first IMU measurement frame will indicate the direction of the first rotation axis 7. The measurement direction will always be perpendicular to the first rotation axis 7. This means that the direction can be used to measure the orientation of the second rotation axis 8 relative to the first IMU measurement frame as well as the vector pointing from the origin of the first IMU measurement frame to the first rotation axis 7.
By assuming that the orientation of the first IMU 12 is being tracked relative to gravity, then the gravitational component of the acceleration R*g can be removed from the sensed motion acceleration ASensed by assuming the measurement error is negligible. In this way, the dynamic component ADynamic of the centripetal acceleration AC can be isolated.
A
Dynamic
=A
Sensed
−R*g
During steady state swinging motions (rotation of the swing boom 6 around the second rotation axis 8) in the absence of other actuations, this dynamic acceleration ADynamic is entirely due to swing motion.
A
Dynamic=ω2×ω2×ρ2
Since ADynamic is always perpendicular to the second rotation axis 8 and points towards the second rotation axis 8 and we know that the Y axis of the swing boom 6 is always parallel to the XY plane for the rotating upper part 4, and thus perpendicular to the second rotation axis 8, a cross product can be used to determine the vector of the second rotation axis v2.
This measurement can then be nondimensionalized and the rotation between the first IMU measurement frame and the upper part frame using the X (u2(x)) and Z (u2(z)) components of the direction of the second rotation axis u2 in the first IMU measurement frame can be calculated.
The swing boom angle θSB can be confirmed and compared to the relative orientation calculated from the orientation of the first IMU 12 and the second IMU with respect to gravity.
There is a second possibility which can be used to calculate the swing boom angle θSB. The distance from the first IMU 12 to the second rotation axis 8 in the rotating upper part horizontal plane can be directly measured or the dynamic component ADynamic of the centripetal acceleration AC from the first IMU measurement frame can be converted into the rotating upper part coordinate frame and the direction in the horizontal plane along with the estimated boom elevation angle can be used to estimate the swing boom angle θSB.
The distance from the second rotation axis to the first IMU ρ2 (dependent on the position of the first IMU on the swing boom) can be expressed in the coordinates of the rotating upper frame and can be written as:
ρ2=ρ3+R4(ρ4+R5ρ5)
It is known that the vertical components of ρ3 (distance between the first rotation axis 7 and the horizontal third rotation axis 9) and ρ4 (distance between the second 8 and the third rotation axis 9) are irrelevant as they lie along the first rotation axis direction, and thus do not contribute to the dynamic component ADynamic of the centripetal acceleration AC. The mounting position of the first IMU 12 with respect to the second rotation axis 8 can also be measured as ρ2 and expressed in the first IMU measurement frame. With this information the X and Y components of ρS (distance between the first IMU and the rotation center) can be calculated and expressed in body coordinates using rotation matrices.
Now the geometry can be related to the measured X and Y distances, noting that:
For instance, assuming the second rotation axis 8 is located 1 m forward and 50 mm to the left of the first rotation axis 7, that the third rotation axis 9 is 100 mm forward and 100 mm above the second rotation axis 8, and that the first IMU 12 is mounted 1 m along its X-axis, 150 mm along its Y-axis, and 250 mm along its Z-axis from the third rotation axis 9 on the side of the swing boom 6, the resulting distances from the second rotation axis 9 to the first IMU 12 in the XY plane of the upper part can be determined. A range of motion of the swing boom rotation about the third rotation axis 9 from −30 to 60 degrees and a range of motion of the swing boom rotation about the second rotation axis 8 of +/−45-degree can be assumed.
Regardless of which possibility is used, the swing boom angle 2 can be estimated by isolating the centripetal acceleration AC experienced by the first IMU 12, calculating the direction of the vector to the second rotation axis 8, the location of the second rotation axis 8, or both, and then cross referencing that with the angle of the swing boom rotation about the third rotation axis 9.
Although aspects are illustrated above, partly with reference to some preferred embodiments, it must be understood that numerous modifications and combinations of different features of the embodiments can be made. All of these modifications lie within the scope of the appended claims.
| Number | Date | Country | Kind |
|---|---|---|---|
| 22180529.4 | Jun 2022 | EP | regional |
