Improved determination of swing boom angle for excavator based on angular velocity ratio

Abstract

The invention relates to improved determination of a swing boom angle of an excavator based on a ratio of angular velocities. The invention relates to a system for determining a swing boom angle of an excavator, the excavator comprising a vehicle frame, a swing boom, and a pneumatic or hydraulic cylinder. A first end of the cylinder is coupled to the vehicle frame and a second end of the cylinder is coupled to the swing boom, the cylinder being configured to rotate the swing boom. The system comprises a first inertial measurement unit (IMU) configured to be mounted on the swing boom and to generate first IMU data, a second IMU configured to be mounted on the cylinder and to generate second IMU data, and a processing unit. The processing unit is configured to receive the first and second IMU data, to determine, when the cylinder is actuated, a first angular velocity of the swing boom about a first axis of rotation based on the first IMU data, and a second angular velocity of the cylinder about a second axis of rotation based on the second IMU data, and to determine the swing boom angle based on a ratio of the second angular velocity to the first angular velocity.

Description

Technical Field

[0001] This invention relates to a system for determining the angle of a swing boom of an excavator, wherein the excavator includes: a frame including a cabin; a swing boom disposed on the frame and configured to rotate relative to the frame about a first axis of rotation; and a pneumatic or hydraulic cylinder. A first end of the pneumatic or hydraulic cylinder is connected to the frame, and a second end is connected to the swing boom, wherein the cylinder is configured to rotate at the first end relative to the frame about a second axis of rotation, and wherein the cylinder is configured to rotate the swing boom. Background Technology

[0002] A large excavator is an earthmoving machine that consists of the following components: a lower section that rests on the ground and is supported by rails or wheels to move the excavator; a rotating upper section that is mounted to the lower section via a large bearing having a vertical axis of rotation; and a mining structure that consists of several components attached to the rotating upper section and connected to each other via linkage pins having horizontal axes of rotation that are parallel to each other.

[0003] Mini excavators can also swing their mining structure relative to the rotating upper part. A pin with a vertical axis of rotation exists between the mining structure and the rotating upper part. This type of construction is called a swing boom because the boom swings relative to the upper part, thus creating a swing boom angle.

[0004] A backhoe is an earthmoving machine consisting of a wheel-supported chassis attached to a loader's excavation structure and an excavating structure. Both excavation structures are composed of several components forming a series of kinematic links that end in excavation tools. A system exists to track the movement of these kinematic links. However, the excavating structure is attached to the chassis via a joint that allows the entire excavating structure to rotate about the vertical direction relative to the chassis.

[0005] Typical sensing algorithms rely on integrating the gyroscope to measure positional displacement and using an accelerometer to measure gravity to stabilize the gyroscope integration and detect true orientation over long frames. Over time, the integrated gyroscope measurement will drift due to uncorrected bias and sensor noise. When the axis of rotation is aligned with or nearly aligned with gravity, gravity measurements cannot be used to stabilize the swing arm rotation; therefore, the swing arm angle has no effect on how the accelerometer observes gravity.

[0006] DK180402B1 discloses a wire sensor for measuring the length of a cylinder used to actuate the movement of a swing boom on a miniature excavator with a swing boom function. The wire sensor is a self-retracting spool with an angle encoder mounted to measure the rotation of the spool relative to a housing. In this mounting, the housing is attached to the main upper part of the excavator. The wire extends and is fixed to the swing boom structure. A one-to-one correspondence exists between the angle encoder measurement and the swing boom angle relative to the upper part.

[0007] There is a need to develop a system that accurately determines the swing boom angle of an excavator without relying on integrating gyroscopes and stabilizing the gyroscope integration by measuring gravity with an accelerometer. Furthermore, alternative methods for tracking the swing boom angle are needed that do not rely on line sensors. Summary of the Invention

[0008] Therefore, the object of the present invention is to provide an improved system for determining the swing boom angle of an excavator.

[0009] This invention relates to a system for determining the swing boom angle of an excavator (e.g., a backhoe excavator, a mini excavator, etc.), wherein the excavator includes: a frame including a cab, a swing boom disposed on the frame and configured to rotate relative to the frame about a first axis of rotation, and a pneumatic or hydraulic cylinder. A first end of the pneumatic or hydraulic cylinder is connected to the frame, and a second end is connected to the swing boom, wherein the cylinder is configured to rotate relative to the frame about a second axis of rotation at the first end, and wherein the cylinder is configured to rotate the swing boom (the cylinder is configured such that extension or retraction of the cylinder rotates the swing boom). The system includes: a first inertial measurement unit (IMU) configured to be mounted on the swing boom and generating first IMU data, a second IMU configured to be mounted on the cylinder and generating second IMU data, and a processing unit. The processing unit is configured to: receive first IMU data and second IMU data; when the cylinder is actuated, determine a first angular velocity of the swing arm about the first rotation axis based on the first IMU data; determine a second angular velocity of the cylinder about the second rotation axis based on the second IMU data; and determine the swing arm angle based on the ratio of the second angular velocity to the first angular velocity.

[0010] The disclosed invention provides a system for stabilizing the measurement of a swing boom angle based on an IMU (particularly a gyroscope). The disclosed system includes an algorithm that provides a mechanism for measuring the swing boom angle at any time as the swing boom moves / the swing boom angle changes. This measurement is based on the instantaneous measurement of the angular velocity of the swing boom and the swing boom cylinder. The system can also be applied to mini excavators or is suitable for use on articulated joints, such as those on wheel loaders, electric graders, and articulated dump trucks.

[0011] In a further embodiment, the system further includes a display unit configured to be disposed in a compartment, wherein the display unit is connected to the processing unit. The display unit is configured to provide the operator with a visualization of the boom angle information based on the boom angle.

[0012] The visualization of boom angle information can be achieved in different ways. On one hand, a specific value of the boom angle can be displayed as a number on a monitor. On the other hand, the boom angle can also be displayed through color coding or on a color scale. For example, red can be displayed for the boom position to the left of the center position (zero degrees) (negative angle value), and blue for the boom position to the right of the center position (positive angle value). Gradient changes in the angle values ​​can be achieved by mixing different colors. Furthermore, it is conceivable to display the boom angle via a bar whose length increases as the boom angle value increases. The boom angle can also be displayed via a scale on which a pointer moves according to the movement of the boom (similar to a compass), thus displaying the corresponding boom angle. Other implementations for visualizing boom angle information are also conceivable.

[0013] In another embodiment, the first IMU includes a first gyroscope, and the second IMU includes a second gyroscope, wherein the first gyroscope is configured to provide data regarding the position of the swing arm as first position data, and the second gyroscope is configured to provide data regarding the position of the cylinder as second position data. The first and second position data are provided by the integration of the first and second gyroscopes. The processing unit is configured to receive the first and second position data and determine the swing arm angle based on the first and second position data.

[0014] In a further embodiment, the processing unit is further configured to: receive first position data and second position data, combine the first position data and second position data with first IMU data and second IMU data (first angular velocity and second angular velocity), and determine the swing boom angle based on the data combination.

[0015] In a further embodiment, the combination of the first location data and the second location data with the first IMU data and the second IMU data is performed by a sensor fusion algorithm.

[0016] In a further embodiment, the sensor fusion algorithm is a complementary filter and / or a Kalman filter and / or an iterative root-finding scheme.

[0017] In a further embodiment, the first gyroscope is a three-axis gyroscope.

[0018] In a further embodiment, the second gyroscope is a three-axis gyroscope.

[0019] In a further embodiment, the system further includes a third IMU, wherein the third IMU includes at least one gyroscope configured to be mounted on the chassis and generate third IMU data.

[0020] In a further embodiment, the processing unit is further configured to:

[0021] • Based on the third IMU data, determine the third angular velocity caused by the rotation of the frame about the fourth rotation axis.

[0022] The fourth angular velocity is determined by subtracting the third angular velocity from the first angular velocity.

[0023] The fifth angular velocity is determined by subtracting the third angular velocity from the second angular velocity.

[0024] • Determine the ratio of the fifth angular velocity to the fourth angular velocity.

[0025] The angle of the swing arm is determined based on the angular velocity ratio.

[0026] The processing unit is also configured to receive first IMU data and third IMU data, determine a first angular velocity of the swing arm about a first rotation axis based on the first IMU data, determine a third angular velocity of the frame about a fourth rotation axis based on the third IMU data, and determine the swing arm angle based on the ratio of the third angular velocity to the first angular velocity.

[0027] In another embodiment, the determination of the swing arm angle based on the angular velocity ratio is accomplished using a 1D lookup table.

[0028] In another embodiment, the determination of the swing arm angle based on the angular velocity ratio is accomplished using a kinematic model consisting of the frame (particularly the cabin), the swing arm, and the cylinder.

[0029] In another embodiment, the first IMU, the second IMU, and the third IMU each include an IMU measurement frame, the first IMU data, the second IMU data, and the third IMU data each include a coordinate frame associated with the IMU measurement frame, and the first IMU data, the second IMU data, and the third IMU data are transformed into a single common coordinate frame, one axis of which is aligned with the first rotation axis.

[0030] In another embodiment, the swing arm rotates relative to the frame about a third rotation axis, wherein the third rotation axis is perpendicular to the first rotation axis, and wherein the processing unit is further configured to:

[0031] The sixth angular velocity of the swing arm about the third rotation axis is determined based on the first IMU data.

[0032] The elevation angle of the swing boom is determined based on the first position data.

[0033] • Based on the sixth angular velocity and the elevation angle, the first IMU data is converted into a single common coordinate frame whose axis is aligned with the first rotation axis.

[0034] In another embodiment, the system is configured to perform self-calibration based on a self-calibration process, the self-calibration process including the following steps:

[0035] • Move the swing arm to the first known position.

[0036] • Move the swing arm to the second known position.

[0037] • During the movement of the swing arm between the first known position and the second known position, the measurement results from the first IMU and the second IMU are recorded as first IMU calibration data and second IMU calibration data.

[0038] The first angular velocity of the swing arm about the first rotation axis is determined based on the first IMU calibration data, and the second angular velocity of the cylinder about the second rotation axis is determined based on the second IMU calibration data.

[0039] The ratio of the second angular velocity to the first angular velocity is determined as the calibration angular velocity ratio.

[0040] • Generate calibration data regarding the relationship between the calibration angular velocity ratio and the swing boom angle. Attached Figure Description

[0041] The invention will now be illustrated in more detail by way of example only, with reference to the schematic examples shown in the accompanying drawings. In the drawings, the same elements are labeled with the same reference numerals. The described embodiments are generally not shown to scale and should not be construed as limiting the invention.

[0042] Figure 1 A schematic illustration of a swing-arm mini excavator is shown in a side view and from above.

[0043] Figure 2 A schematic illustration of the swing arm joint from top to bottom is shown, with the swing arm in the middle position.

[0044] Figure 3 A schematic illustration of the swing arm joint from top to bottom is shown, with the swing arm in the left-hand moving position.

[0045] Figure 4 An example graph is shown, which illustrates the kinematic relationship of the fixed geometry of the swing boom cylinder's motion ring. Detailed Implementation

[0046] Figure 1 A schematic illustration of an excavator 1 is shown in a side view and from above, illustrating the various components of the excavator 1. The excavator 1 includes a frame 3 with a compartment 4, a swing boom 5, and pneumatic or hydraulic cylinders 6. The swing boom 5 is arranged on the frame 3 via a swing boom joint 7 and can rotate relative to the frame 3 about a vertical first axis of rotation 8 (this degree of freedom determines the swing boom angle θ). SB The swing boom 5 can also be rotated about a horizontal third rotation axis 9 to raise or lower the swing boom 5 (this degree of freedom determines the so-called elevation angle / boom angle 10). In the example shown, the boom 11 and bucket 12 are attached to the swing boom 5, which can also be moved via horizontal rotation axes. These horizontal rotation axes are parallel to the third rotation axis 9.

[0047] Figure 2 A top-down schematic illustration of the swing arm joint 7 is shown, with the swing arm 5 in a centered / intermediate position. The cylinder 6 and the swing arm 5 are mounted on the frame 3, wherein in this example, the swing arm 5 is mounted on the frame 3 via the joint 7. A first end of the cylinder 6 is connected to the frame 3, and a second end is connected to the swing arm 5 via the joint 7, wherein the distance between the second rotation axis 15 at the first end and the attachment position 16 of the cylinder 6 on the swing arm joint 7 is indicated by the pointing vector R′. C (Equal to cylinder length) is represented. The distance between the second rotation axis 15 and the first rotation axis 8 is indicated by the pointing vector R. VThe distance between the first rotation axis 8 and the attachment position 16 of the cylinder 6 on the swing arm joint 7 is indicated by the pointing vector R′. SB Indicated. Cylinder 6 rotates at its first end relative to the frame 3 about a second axis of rotation 15. This is achieved by an angular offset θ depending on the manufactured geometry. 偏移 The centerline X of the swing boom and the angle θ of the swing boom SB Related. Furthermore, θ SB It is R′ C With R′ SB The angle between them, R′ C With R′ SB Both are extensions. θ V It is R V With R′ C The angle between them.

[0048] When cylinder 6 rotates relative to frame 3 about the second rotation axis 15, cylinder 6 extends or retracts depending on the direction of rotation. The joint 7 and the attached swing arm 5 rotate, which causes the center line X of the swing arm (the longitudinal axis of the swing arm 5 relative to the first rotation axis 8) to rotate relative to frame 3 (the longitudinal axis A of frame 3 relative to the first rotation axis 8).

[0049] Figure 3 A top-down schematic illustration of the swing arm engagement 7 is shown, with the swing arm 5 in a moved left-side position. Cylinder 6 extends due to its counter-clockwise rotation about the second rotation axis 15. The actuated movement of cylinder 6 presses against engagement 7, which also rotates counter-clockwise about the first rotation axis 8, causing the swing arm 5 (centerline X) to also rotate counter-clockwise and remain in the left-side position on axis A. When cylinder 6 rotates clockwise about the second rotation axis 15, cylinder 6 retracts, causing engagement 7 and the swing arm 5 to also move clockwise (remaining in the right-side position on axis A). The rotation of cylinder 6 causes the second rotation axis 15 and cylinder 6 to be aligned by the pointing vector R. C The direction of the distance between the attachment positions 16 on the swing arm joint 7 is changed. (By R) C and R′ C The angle θ formed C The rotation angle of cylinder 6 about the second rotation axis 15 is represented by this angle. The rotation of the joint 7 changes the position of the joint 7 relative to the frame 3, resulting in R′. SB To R SB The direction changes. This also leads to R′ C With R SB The angle θ between them SBThe system also includes a first inertial measurement unit (IMU) 17 and a second IMU 18, the first IMU 17 being configured to be mounted on the swing arm 5 and generate first IMU data, and the second IMU 18 being configured to be mounted on the cylinder 6 and generate second IMU data.

[0050] Figure 3 The aforementioned vectors and angles are also shown to illustrate the swing boom angle θ. SB The mathematical determination is as follows. For convenience, the angles described are referenced from the horizontal direction (top to bottom), but the equations apply to any reference for the angle as long as it is expressed continuously within the range of the swing arm's movement. Using pointing vectors to represent the length between two points in the structure, the equations for the vector loop can be written:

[0051] R V +R SB —R C =0.

[0052] This is the plane equation written in the XY plane of the vehicle frame. Expanding this equation to include the X and Y component equations yields:

[0053]

[0054] The swing arm movement (rotation about the first axis of rotation 8) has one degree of freedom. Because there is only one degree of freedom in this kinematic chain, only one velocity (cylinder length change velocity, cylinder angular velocity, or swing arm angular velocity) is independent. The other two velocities are functions of the independent velocities and the position of the kinematic chain within its range of motion. Therefore, if the first angular velocity ω SB Second angular velocity ω C If all measurements are taken, sufficient information can be used to calculate the position of the kinematic chain within its range of motion. Knowing the cylinder length R... C This enables the calculation of the swing arm angle θ. SB And conversely, knowing the swing arm angle θ SB Allowable calculation of cylinder length R C These vector equations can be expressed relative to the cylinder length R. C Differentiate the changes:

[0055]

[0056] We can now define the dependent variable, the angle relative to the independent variable, and the cylinder length R. C The derivative:

[0057]

[0058]

[0059] Wherein, the first angular velocity ω SB Second angular velocity ω C Relative to frame 3, this is expressed as a function of the motion ratio and cylinder speed, where positive cylinder speed is extension and negative pneumatic cylinder speed is retraction. This leads to a set of equations that can be used to calculate the motion ratio:

[0060]

[0061] Several methods, such as Cramer's rule and back substitution, can be used to solve this system of equations. The solution obtained is:

[0062]

[0063]

[0064] As shown in the figure, the angular motion θ′ of the swing arm SB Angular motion θ′ of the cylinder C It is the cylinder length R C and two vectors R SB and R C The angle between them (θ) C ―θ SB The function of ).

[0065]

[0066] This can be achieved by using the cylinder angular velocity ω. C With the angular velocity ω of the swing arm SB The ratio is used to expand. Then the interior angles (θ) of the vector ring triangle can be calculated. C ―θ SB The cosine of ).

[0067]

[0068] It can also use the law of cosines to relate the cylinder length R. C The length of the base (the distance between the second rotation axis 15 and the first rotation axis 8) R V and swing boom structure R SB (the distance between the first rotation axis 8 and the attachment position 16 of the cylinder 6 on the swing arm joint 7) and the interior angle (θ) C ―θ SB Related:

[0069]

[0070] Convert the interior angle (θ C ―θ SB Substituting the cosine of ) into the equation above, we get:

[0071]

[0072] This can be used as a ratio of angular velocity. The function of cylinder length R C Solve the problem.

[0073]

[0074] And substitute this into the solution to find the angle (θ) C ―θ SB )get:

[0075]

[0076] Now we can use cylinder angular velocity ω C With the angular velocity ω of the swing arm SB The ratio is used to track the motion of the swing arm 5. This process is as follows:

[0077] 1. Track the rotation of the swing arm 5 around the first rotation axis 8.

[0078] 2. Use the rotating swing arm to measure the angular velocity ω of the swing arm. SB It is decomposed into vertical components and horizontal components.

[0079] 3. Measure the angular velocity ω of the swing boom cylinder about the second rotation axis 15 (parallel to the Z-axis of the frame). C .

[0080] 4. Measure the third trigonometric velocity ω of the frame about the fourth rotation axis 19 (parallel to the frame's Z-axis). V .

[0081] 5. Calculate the angular velocity ω of the swing arm. SB and the angular velocity ω of the swing boom cylinder C angular velocity ω relative to the frame V (The third angular velocity caused by the rotation of the frame 3 around the fourth rotation axis 19 based on the third IMU data).

[0082] ω SB =[0 0 1](R) SB ω SB -ω V )

[0083] ω C =[0 0 1](ω c -ω V )

[0084] 6. Apply the derived equation to calculate the interior angle (θ) of the swing arm's rotation. C -θ SB ).

[0085] 7. Using any method (sine law, cosine law, 1-D table lookup, etc.) to correlate the orientation of the interior angle with the center line X of the swing arm is relative to the longitudinal axis A of the frame 3.

[0086] Figure 4 An exemplary graph is shown, illustrating the kinematic relationship of a fixed geometry of the swing boom cylinder motion ring. The cylinder length controls the kinematic motion of the ring. Curve (A) shows the motion ratio (the ratio of the second angular velocity to the first angular velocity) as a function of cylinder length (cylinder extension level). Curve (B) shows the interior angle as a function of cylinder length (pneumatic cylinder extension level). However, since there is a single degree of freedom in this kinematic chain, knowing any one of the cylinder length, interior angle, and motion ratio is sufficient to fully define the configuration. Knowing the motion ratio allows the cylinder length to be calculated, which in turn allows the interior angle to be calculated. Graphically, this corresponds to starting with the motion ratio and finding the point where the measured motion ratio intersects the curve of the motion ratio as a function of cylinder position (curve (A)). This corresponds to a horizontal line on the graph. The next step is to determine the point where the cylinder length intersects the curve of the interior angle as a function of cylinder position (curve (B)). This corresponds to a vertical line on the graph. The resulting interior angle corresponds to the measured motion ratio. This is determined using another horizontal line on the graph.

[0087] Although a graphical approach has been discussed, this is equivalent to having a 1D lookup table or analytical equation to convert the motion ratio into cylinder length, and a 1D lookup table or analytical equation to convert the cylinder length into interior angle. If the cylinder length is only used as an intermediate value and not for any other purpose, a single 1D lookup table or analytical equation can be used to directly convert the measured motion ratio into the corresponding interior angle.

[0088] In the vehicle's XY coordinates, the basis vector R V The value is [750300] mm. The swing arm vector R... SB In the swing boom coordinate system, it is [100-300] mm. Cylinder length R C Within the range of 687mm to 990mm, and with a swing boom angle θ SB It varies between -30 degrees and +30 degrees. Interior angle (θ) C -θ SB Both the motion ratio and the cylinder length R are related. C One-to-one correspondence.

[0089] The angle estimate provided by this method can be combined with the estimate determined by the gyroscope integral in the sensor fusion algorithm. Any suitable algorithm can be used, such as complementary filters, Kalman filters, iterative root-finding schemes, etc.

[0090] To limit the effects of misalignment in the second (swing boom cylinder 6) gyroscope 20, a 3-axis gyroscope can be used. The swing boom 5 can then be actuated several times, and a calibration rotation can be calculated, which will measure the second angular velocity ω. C Align with the Z-axis of the measuring frame.

[0091] Instead of transforming the 3-axis gyroscope measurements from the first IMU 17 into the horizontal plane, a single-axis gyroscope can be mounted on the swing arm 5 to directly measure the rotational speed about the first rotation axis 8. As in the case of cylinder gyroscope 20, a 3-axis gyroscope can be used, and the calibration rotation can be calculated to ensure that the measuring frame is aligned with the Z-axis of the chassis.

[0092] If cylinder 6 is equipped with a sensor for speed measurement, the described method can be applied using the ratio of the swing arm angular velocity to the pneumatic cylinder speed and the sine law to determine R. SB and R C The interior angle (θ) between C ―θ SB It is related to the angle between A and X.

[0093]

[0094]

[0095] Note that this method must be used with caution because the arcsine has a range of -90 to 90 degrees and cannot distinguish between interior angles of 80 degrees and 100 degrees. Logic should be included to ensure the correct angle is used near 90 degrees, and gyroscope measurements are used to properly manage the solution when it passes through 90 degrees.

[0096] Although the invention has been illustrated above with reference to some preferred embodiments, it should be understood that various modifications and combinations of different features of the embodiments are possible. All such modifications are within the scope of the appended claims.

Claims

1. A system for determining the swing boom angle of an excavator (1), wherein, The excavator (1) includes: The chassis (3) includes a compartment (4). A swing arm (5) is arranged on the frame (3) and configured to rotate relative to the frame (3) about a first axis of rotation (8). A pneumatic or hydraulic cylinder (6) is provided, the first end of which is connected to the frame (3) and the second end of which is connected to the swing arm (5). The cylinder (6) is configured to rotate relative to the frame (3) about a second rotation axis (15) at the first end, wherein the cylinder (6) is configured to rotate the swing arm (5). The system is characterized in that it comprises: A first inertial measurement unit (17) is configured to be mounted on the swing arm (5) and generate first inertial measurement unit data. A second inertial measurement unit (18) is configured to be mounted on the cylinder (6) and generate second inertial measurement unit data. Processing unit, the processing unit being configured to: Receive data from the first inertial measurement unit and data from the second inertial measurement unit. When the cylinder (6) is actuated, the first angular velocity of the swing arm (5) about the first rotation axis (8) is determined based on the data from the first inertial measurement unit, and the second angular velocity of the cylinder (6) about the second rotation axis (15) is determined based on the data from the second inertial measurement unit. The swing arm angle is determined based on the ratio of the second angular velocity to the first angular velocity.

2. The system according to claim 1, wherein, The system further includes a display unit configured to be arranged in the compartment (4), wherein the display unit is connected to the processing unit and is configured to provide the operator with a visualization of the swing boom angle information based on the swing boom angle.

3. The system according to claim 1, wherein, The first inertial measurement unit (17) includes a first gyroscope, and the second inertial measurement unit (18) includes a second gyroscope, wherein the first gyroscope is configured to provide data about the position of the swing arm (5) as first position data, and the second gyroscope is configured to provide data about the position of the cylinder (6) as second position data.

4. The system according to claim 3, wherein, The processing unit is further configured to: Receive the first location data and the second location data. The first position data and the second position data are combined with the first inertial measurement unit data and the second inertial measurement unit data. The angle of the swing arm is determined based on the combination of data.

5. The system according to claim 4, wherein, The combination of the first position data and the second position data with the first inertial measurement unit data and the second inertial measurement unit data is performed by a sensor fusion algorithm.

6. The system according to claim 5, wherein, The sensor fusion algorithm is as follows: Complementary filters, and / or Kalman filter, and / or Iterative root-finding scheme.

7. The system according to claim 3, wherein, The first gyroscope is a three-axis gyroscope.

8. The system according to claim 3, wherein, The second gyroscope is a three-axis gyroscope.

9. The system according to any one of claims 3 to 8, wherein the system further comprises a third inertial measurement unit, wherein, The third inertial measurement unit includes at least one gyroscope, which is configured to be mounted on the vehicle frame and generate third inertial measurement unit data.

10. The system according to claim 9, wherein, The processing unit is further configured to: The third angular velocity caused by the rotation of the frame (3) about the fourth rotation axis is determined based on the data from the third inertial measurement unit. The fourth angular velocity is determined by subtracting the third angular velocity from the first angular velocity. The fifth angular velocity is determined by subtracting the third angular velocity from the second angular velocity. Determine the ratio of the fifth angular velocity to the fourth angular velocity. The angle of the swing arm is determined based on this angular velocity ratio.

11. The system according to claim 10, wherein, The determination of the swing boom angle based on the angular velocity ratio is accomplished using a 1D lookup table.

12. The system according to claim 10, wherein, The determination of the swing arm angle based on the angular velocity ratio is accomplished using a kinematic model, which consists of the frame (3), the swing arm (5), and the cylinder (6).

13. The system according to claim 12, wherein, The kinematic model consists of the compartment (4), the swing arm (5), and the cylinder (6).

14. The system according to claim 9, wherein, The first inertial measurement unit (17), the second inertial measurement unit (18), and the third inertial measurement unit each include an inertial measurement unit measurement frame. The first inertial measurement unit (IMU) data, the second IMU data, and the third IMU data each include a coordinate frame associated with the IMU measurement frame. The data from the first inertial measurement unit, the second inertial measurement unit, and the third inertial measurement unit are transformed into a single common coordinate frame, with one axis aligned with the first rotation axis (8).

15. The system according to claim 14, wherein, The swing arm (5) rotates relative to the frame (3) about a third rotation axis (9), wherein the third rotation axis (9) is perpendicular to the first rotation axis (8), and wherein the processing unit is further configured to: The sixth angular velocity of the swing arm (5) about the third rotation axis (9) is determined based on the data from the first inertial measurement unit. The elevation angle (10) of the swing arm (5) is determined based on the first position data. The first inertial measurement unit data is converted into a single common coordinate frame whose axis is aligned with the first rotation axis (8) based on the sixth angular velocity and the elevation angle (10).

16. The system according to any one of claims 1 to 8, wherein, The system is configured to perform self-calibration based on a self-calibration process, which includes the following steps: Move the swing arm (5) to the first known position. Move the swing arm (5) to the second known position. During the movement of the swing arm (5) between the first known position and the second known position, the measurement results from the first inertial measurement unit (17) and the second inertial measurement unit (18) are recorded as first inertial measurement unit calibration data and second inertial measurement unit calibration data. The first angular velocity of the swing arm (5) about the first rotation axis (8) is determined based on the calibration data of the first inertial measurement unit, and the second angular velocity of the cylinder (6) about the second rotation axis (15) is determined based on the calibration data of the second inertial measurement unit. The ratio of the second angular velocity to the first angular velocity is determined as the calibration angular velocity ratio. Generate calibration data regarding the relationship between the calibration angular velocity ratio and the swing boom angle.

Images