An IMU-based system for vertical axis joint angle estimation of a swing-boom excavator

Abstract

A system and method are provided for determining a swing angle of a swing boom of a vehicle. Sensor data is received from sensors disposed on the swing boom and vehicle body of the vehicle. It is determined whether the swing boom is stationary or moving relative to the vehicle body based on the sensor data. If it is determined that the swing boom is stationary, the received sensor data is corrected based on an observed swing angle, and an estimated swing angle is calculated based on the corrected sensor data. If it is determined that the swing boom is moving, an estimated swing angle is calculated based on the received sensor data. The estimated swing angle is output.

Description

[Technical Field]

[0001] The present invention relates generally to vertical axis joint angle estimation for swing boom excavators, and more particularly to an IMU (inertial measurement unit) based system for vertical axis joint angle estimation for swing boom excavators. [Background technology]

[0002] Guidance systems and automatic position control systems are becoming increasingly common in construction, mining, and agricultural machinery. For example, in swing boom excavators, guidance systems can improve operator awareness, and automatic position control systems can alleviate some of the complexities of steering the excavator and positioning the swing boom. To implement such guidance and automatic position control systems, it is important to accurately estimate the joint angles between each joint of the swing boom.

[0003] In one conventional method, gyroscopes are used to measure motion and determine the angle between the boom joints. However, such conventional gyroscopes suffer from drift issues due to gyroscope bias. Accelerometers are sometimes used to correct for such drift. However, accelerometers cannot adequately correct gyroscope bias for vertical or near-vertical joints because, unlike axes projected onto a sufficiently large horizontal axis, the accelerometer output remains the same as the joint angle changes. Summary of the Invention

[0004] In one or more embodiments, a system and method are provided for determining a swing angle of a swing boom of a vehicle. Sensor data is received from sensors disposed on a vehicle body and a swing boom. Based on the sensor data, it is determined whether the swing boom is stationary or moving relative to the vehicle body. If it is determined that the swing boom is stationary, the received sensor data is corrected based on an observed swing angle, and an estimated swing angle is calculated based on the corrected sensor data. If it is determined that the swing boom is moving, the estimated swing angle is calculated based on the received sensor data. The estimated swing angle is output.

[0005] In one embodiment, it is determined whether the swing boom is stationary or moving relative to the vehicle body by calculating the energy of the signal received from the sensor, comparing the calculated energy to one or more threshold values, and determining whether the swing boom is stationary or moving based on the comparison.

[0006] In one embodiment, correcting the received sensor data comprises comparing the observed swing angle to a most recent estimated swing angle and removing bias from the sensor data based on the comparison.

[0007] In one embodiment, a determination is made that the estimated swing angle exceeds a swing limit for the vehicle and a Kalman filter used to calculate the estimated swing angle is reset.

[0008] In one embodiment, the observed swing angle is calculated by determining a rotation axis of the sensor and calculating the observed swing angle based on the determined rotation axis. In another embodiment, the observed swing angle is calculated by determining a swing angle error by forward alignment and calculating the observed swing angle based on the swing angle error. In another embodiment, the observed swing angle is calculated by calculating the observed swing angle based on roll and pitch of the vehicle when it is determined that the vehicle is positioned on a lean that satisfies a lean threshold.

[0009] In one embodiment, the sensor is an inertial measurement unit (IMU).In one embodiment, the vehicle is an excavator.

[0010] These and other advantages of the present invention will become apparent to those of ordinary skill in the art upon review of the following detailed description and accompanying drawings. [Brief explanation of the drawings]

[0011] [Figure 1] FIG. 1 illustrates a typical excavator in which one or more embodiments described herein may be implemented. [Figure 2] FIG. 2 illustrates a top view of an excavator in accordance with one or more embodiments. [Figure 3] FIG. 3 illustrates a workflow for calibrating an IMU located on an excavator in accordance with one or more embodiments. [Figure 4] FIG. 4 illustrates a schematic diagram of a swing angle estimation system for estimating the swing angle of an excavator in accordance with one or more embodiments. [Figure 5] FIG. 5 illustrates a schematic diagram of a kinematic angle estimation module in accordance with one or more embodiments. [Figure 6] FIG. 6 illustrates a method for estimating the swing angle of an excavator in one or more embodiments. [Figure 7]FIG. 7 illustrates a hysteresis-type threshold for determining whether the swing boom is stationary or moving, in one or more embodiments. [Figure 8] FIG. 8 illustrates a state machine for determining the swing angle of a swinging boom of a vehicle in accordance with one or more embodiments. [Figure 9] FIG. 9 illustrates an exemplary user interface according to one or more embodiments. [Figure 10] FIG. 10 illustrates a model of an excavator in accordance with one or more embodiments. [Figure 11] FIG. 11 shows a graph of a raw gyroscope signal recorded on an excavator in accordance with one or more embodiments. [Figure 12] FIG. 12 shows another graph of a raw gyroscope signal in accordance with one or more embodiments. [Figure 13] FIG. 13 illustrates a model of a test fixture according to one or more embodiments. [Figure 14] FIG. 14 illustrates a graph that is an example output of a damped vibration rotation axis estimation algorithm according to one or more embodiments. [Figure 15] FIG. 15 illustrates a graph showing the change in axis of rotation as the swing IMU rotates, according to one or more embodiments. [Figure 16] FIG. 16 illustrates a plot of signals for calculating the axis of rotation in accordance with one or more embodiments. [Figure 17] FIG. 17 shows a graph that is an example of the application of s to a function f, in accordance with one or more embodiments. [Figure 18] FIG. 18 shows a schematic diagram of an error estimation module according to one or more embodiments. [Figure 19] FIG. 19 shows a graph illustrating the results of forward alignment in accordance with one or more embodiments. [Figure 20] FIG. 20 shows a high-level block diagram of a computer that can be used to implement one or more embodiments. Detailed Description of the Invention

[0012] The embodiments described herein provide an IMU (inertial measurement unit)-based swing angle estimation system. A data fusion swing angle system is provided that combines multiple algorithms to estimate the swing angle of a swing boom relative to the vehicle's chassis using only sensor data from an IMU located on the vehicle. Specifically, the algorithms include kinematic angle estimation (where hypothesis testing is used to determine static dynamic epochs), rotation axis calculation, transfer alignment (using machine residual vibration), and non-zero tilt. An embodiment is also disclosed that uses existing sensors located on the boom, eliminating the need for an additional IMU on the swing link. The estimated swing angle can be used in the excavator's guidance system and / or automatic control system. Knowing this angle is important for optimizing the use of excavators with this additional feature, especially if the operator is allowed to change the angle. The vehicle may be, for example, a construction machine, a mining machine, an agricultural machine, or the like. In one embodiment, the vehicle is an excavator, as shown in FIG. 1.

[0013] FIG. 1 illustrates an exemplary excavator 100 capable of implementing one or more embodiments described herein. While the excavator 100 of FIG. 1 is depicted as a compact or mini excavator, it may be implemented in any suitable form. The excavator 100 includes a body (i.e., house) 104 for accommodating an operator and other components, a chassis 102 with a walkway for guiding the excavator 100 and support 104, and a boom swing 106 coupled to a bucket 108 for manipulating soil or other material. The excavator 100 may additionally or alternatively include other components, such as a blade for performing dozing operations. During operation, the excavator 100 rotates at one or more joints 108-A, 108-B, 108-C, 108-D, and 108-E (collectively "joints 108") to perform various tasks, such as digging, dumping, and manipulating soil or other materials.

[0014] As shown in FIG. 1 , the multiple IMUs are comprised of IMU 110-A located on bucket 108, IMUs 110-B, 110-C, and 110-D located on boom swing 106, and IMU 110-E located on vehicle body 104 (collectively referred to as "IMUs 110"). In one embodiment, each IMU 110 includes a three-axis gyroscope and a three-axis accelerometer. However, each IMU 110 may be implemented as any suitable sensor or sensors for measuring angular velocity and acceleration. For example, IMUs 110 may instead be implemented as separate gyroscopes and separate accelerometers.

[0015] In embodiments described herein, the swing angle at joint 108-D of excavator 100 is determined based solely on information from IMU 110-D and IMU 110-E. The swing angle is the angle of swing boom 106 relative to a reference plane when viewed from above excavator 100, and is shown in FIG. 1 as the angle of joint 108-D. The swing angle is also shown in FIG. 2, as described in more detail below. The angles of other joints (e.g., joint 108-E) may also be determined in accordance with embodiments described herein. The angle of joint 110 may be used, for example, to provide guidance to an operator of excavator 100 and / or for automated control of excavator 100.

[0016] It should be understood that the number and placement of IMUs 110 in the excavator 100 shown in FIG. 1 are exemplary and may vary based on the application. For example, in one application, the embodiments described herein may be implemented as a standalone dual-IMU swing angle guidance system using an IMU 110-E located on the carbody 104 and an IMU 110-D located on the boom swing 106 to determine the swing angle at joint 108-D, as described above. In another exemplary application, the embodiments described herein may be integrated into an existing control and / or guidance system to function as a virtual sensor. In another application, the embodiments described herein may utilize an existing IMU in the boom swing 106 to reduce costs. In another exemplary application, the embodiments described herein may receive as an input the swing rotation angle of the carbody 104 (which may be derived using a GPS-based attitude estimation subsystem) to improve performance when the excavator 100 is operating on a slope.

[0017] A typical operational scenario for excavator 100 is as follows: 1) align boom swing 106 in a first direction; 2) steer boom swing 106, e.g., for "digging"; 3) align boom swing 106 in a second direction; 4) steer boom swing 106, e.g., for "release"; 5) return to step 1 and repeat the cycle. This operational scenario may be achieved by: 1) swinging boom swing 106 to a specific arm angle and pivoting between the first and second directions while keeping the swing constant; 2) pivoting to a specific house angle and pivoting boom swing 106 between the first and second directions while keeping the pivot constant; or 3) a combination of 1 and 2. When using a dual IMU system, the goal is to estimate the swing angle at joint 108-D using only information from IMUs 110-D and 110-E (i.e., rotational rates about their three axes and specific forces along those axes).

[0018] FIG. 2 is a top view of an excavator 200 according to one or more embodiments. In one example, the excavator 200 is the excavator 100 of FIG. 1. The excavator 200 includes a carbody 202 and a swing boom 204. The swing angle of the excavator 200 is the angle that the swing boom 204 forms with respect to a reference plane 206 when viewed from above the excavator 200. The reference plane 206 is shown in FIG. 2 as the center plane of the excavator 200, but may be any other suitable reference plane. In FIG. 2, the excavator 200 is in a reset or zero position, with the swing boom 204 aligned along the front of the carbody 202 (i.e., along the reference plane 206), thus providing a swing angle of zero degrees. The swing boom 204 can rotate clockwise and counterclockwise within swing limits relative to the reference plane 206, for example, between −40° and +40°. The swing limit may be any suitable angle and may be implemented by a mechanical stop.

[0019] FIG. 3 illustrates a workflow 300 for calibrating IMUs 308, 310 located on an excavator 302 in accordance with one or more embodiments. In one example, the excavator 302 may be the excavator 100 of FIG. 1. As shown in FIG. 3, the swing IMU 308 is located on the boom swing 304 of the excavator 302, and the carbody IMU 310 is located on the carbody 306 of the excavator 302. The IMUs 308, 310 are calibrated to compensate for nonlinearities, scale factors, gyroscope bias, axis misalignment, and temperature variations. Additionally, the IMUs 308, 310 are geometrically calibrated to ensure that the geometry of the excavator 302 and the placement of the IMUs 308, 310 are accurately measured.

[0020] The following measurements are important: 1) a position measurement to align the swing boom 304 along the front of the vehicle body 306 (i.e., at the reset or zero attitude), 2) a measurement of the left and right swing limits of the swing boom 304, implemented for example by mechanical stops, and 3) an alignment transformation between the IMUs 308, 310. Measurement 1 is a prerequisite for measurement 3. Once the swing boom 304 is placed in the reset attitude, the rotation matrix that aligns the IMU 308 relative to the IMU 310 can be accurately estimated.

[0021] As shown in workflow 300, the initial axis 312 of IMU 308 is calibrated to calibration axis 316, and the initial axis 314 of IMU 310 is calibrated to calibration axis 318, such that the X-axes of IMUs 308, 310 are aligned along the longitudinal axis (i.e., front-to-back axis) of excavator 302, the Y-axes of IMUs 308, 310 are aligned along the transverse axis (i.e., side-to-side axis) of excavator 302, and the Z-axes of IMUs 308, 310 are aligned along the vertical axis (i.e., up-down axis) of excavator 302. Regardless of the alignment of the initial axes 312, 314, after calibration, the alignment of axes 312, 314 is the same. The swing motion of swing boom 304 can be thought of as rotating swing IMU 308 about the Z-axis, which corresponds to the single degree of freedom axis of the joint connecting swing boom 304 to carbody 306. Formally, the coordinate frame of swing IMU 308 is Vector represented by TIFF0007783400000001.tif811 TIFF0007783400000002.tif811 is in the coordinate frame of the vehicle IMU 310 according to the following formula: It can be represented as TIFF0007783400000003.tif810. TIFF0007783400000004.tif1037 where: TIFF0007783400000005.tif1012 is equal to the following formula: TIFF0007783400000006.tif1750 where θ is the swing angle. In some embodiments, the rotation matrix TIFF0007783400000007.tif1012 shows that mechanical tolerances can be explained by error rotation as follows: TIFF0007783400000008.tif1062 The angles that make up the error matrix are small, resulting in a small rotational nature, which can be treated as additive noise. TIFF0007783400000009.tif1064

[0022] FIG. 4 shows a schematic diagram of a swing angle estimation system 400 for estimating the swing angle of an excavator in one or more embodiments. The swing angle estimation system 400 will be described with continued reference to the excavator 100 of FIG. 1. In one example, the swing angle estimation system 400 is implemented in the excavator 100 shown in FIG. 1 to estimate the swing angle of the swing boom 106. However, the swing angle estimation system 400 may be implemented in other suitable vehicles, such as construction machinery, mining machinery, agricultural machinery, etc. The swing angle estimation system 400 may be implemented by one or more controllers or other suitable processing devices, such as the computer 2002 of FIG. 20.

[0023] The swing angle estimation system 400 estimates the angular velocity of the excavator chassis measured by a chassis IMU 404 (e.g., IMU 110-E in FIG. 1) located on the chassis of the excavator. TIFF0007783400000010.tif811 and acceleration TIFF0007783400000011.tif810 and the angular velocity of the excavator's swing boom measured by the swing IMU 402 (e.g., IMU 110-D in Figure 1) placed on the excavator's swing boom. TIFF0007783400000012.tif812 and acceleration Estimated swing angle based only on TIFF0007783400000013.tif811 TIFF0007783400000014.tif83. The core of the swing angle estimation system 400 is to estimate the observed swing angle θ o The kinematic angle estimation module 406 corrects the sensor data received from the swing IMU 402 and the body IMU 404 based on the observed swing angle θ o may be calculated by the slope estimation module 408, the rotation axis estimation module 412, and / or the error estimation module 414. The error estimation module 414 calculates the angular velocity TIFF0007783400000015.tif812, TIFF0007783400000016.tif811, acceleration TIFF0007783400000017.tif810 and estimated swing angle Observed swing angle θ based on TIFF0007783400000018.tif83 o The rotation axis estimation module 412 determines the rotation axis and, under appropriate conditions (e.g., high vibration, low noise), calculates the observed swing angle θ based on the rotation axis. o The slope estimation module 408 calculates the angular velocity when the excavator is positioned on a surface with a sufficiently large slope and remains stationary for a sufficiently long period of time. TIFF0007783400000019.tif812 and acceleration TIFF0007783400000020.tif811 and the observed swing angle θ using the inclination estimation module 410 based on o The modules of the swing angle estimation system 400 are described in further detail below.

[0024] Kinematic angle estimation

[0025] Figure 5 illustrates a schematic diagram of a kinematic angle estimation module 500 in one or more embodiments. Figure 6 illustrates a method 600 for estimating the swing angle of an excavator in one or more embodiments. Figures 5 and 6 will be described together. In one example, the kinematic angle estimation module 500 is the kinematic angle estimation module 406 of Figure 4, and the steps of method 600 are performed by the kinematic angle estimation module 406 of Figure 4.

[0026] In step 602 of Figure 6, sensor data is received from sensors located on a swing boom and body of a vehicle. The vehicle may be, for example, a construction machine, a mining machine, an agricultural machine, or any other suitable vehicle having a swing boom and body. In one embodiment, the vehicle is the excavator 100 of Figure 1. The sensor data includes the angular velocity of the body, as measured by one or more sensors located on the body of the vehicle. TIFF0007783400000021.tif811, Linear Acceleration TIFF0007783400000022.tif810 and the angular velocity of the swing boom measured by one or more sensors disposed on the swing boom of the vehicle. TIFF0007783400000023.tif812, Linear Acceleration TIFF0007783400000024.tif811. Sensor data is expressed in the local coordinate frame of the sensor that measured it.

[0027] The sensor group may include any suitable sensors for measuring angular velocity and linear acceleration. In one embodiment, the sensor group is an IMU including a gyroscope and an accelerometer. In another embodiment, the sensor group includes separate gyroscopes and accelerometers. In one example, as shown in FIG. 5, the sensor group may include a body IMU 502 located on the body of the vehicle and a swing IMU 504 located on the swing boom of the vehicle. The body IMU 502 and the swing IMU 504 may be calibrated with an offset determined by offset calibrator 506, for example, according to workflow 300 of FIG. 3.

[0028] In step 604 of Figure 6, it is determined whether the swing boom is stationary or moving relative to the vehicle body based on the sensor data. For example, as shown in Figure 5, a ZMD (zero movement detection) module 510 determines whether the swing boom is stationary or moving based on the output of the summer 508. When the swing boom rotates around the swing joint connected to the vehicle body, the swing angle changes because the swing boom is moving relative to the vehicle body.

[0029] The decision as to whether the swing boom is stationary or moving can be made into a hypothesis testing problem in which one of the following two hypotheses must be chosen: (1) Null hypothesis H0: The swing boom is working. (2) Alternative hypothesis H1: The swing boom is stationary.

[0030] The Neyman-Pearson theorem can be applied to provide a threshold for likelihood ratio tests (LRTs). The likelihood ratio is a measure of the likelihood of H0 compared to H1. For a set of observations z, the likelihood ratio is defined as: TIFF0007783400000025.tif2229 p(z|H1) is the probability density function of the corresponding hypothesis. According to the Neyman-Pearson theorem, to maximize the detection probability Pr{H1|H1}, H1 must be chosen if L(z)>γ, considering that the false alarm probability Pr(H1|H0) is equal to α. TIFF0007783400000026.tif1562 Therefore, γ is a threshold determined from Pr{H1|H1}=α.

[0031] The rest of the swing boom does not include all of the motion. For example, in this application example, the rotation speed of the boom swing around the Z axis TIFF0007783400000027.tif814 and the linear acceleration projected onto a plane (where the normal to the plane is the Z axis). Only TIFF0007783400000028.tif819 is targeted, where TIFF0007783400000029.tif82 is the vector from the center of the swing joint to the sensor of the swing boom. Furthermore, the signal of the gyroscope is usually orders of magnitude better than the signal of the accelerometer in terms of quality and noise characteristics. Therefore, only the velocity data (data from the gyroscope sensor) is used to detect the operation. Thus, H1 is selected as follows. TIFF0007783400000030.tif1771 Here, N is the number of samples. The noise variance σ ω of the gyroscope is just a scale factor in this special case and can be ignored. That is, the energy of the gyroscope signal is calculated, and if the calculated energy of the gyroscope signal is below the threshold, it is determined whether the swing joint is stationary or operating. σ ω When being ignored in the formula, the left side of the inequality is the energy of the discontinuous time in the gyroscope signal (in this case ω z,swing ).

[0032] For determining the threshold, a known method for learning the energy of the signal may be used. By such a method, the current (stationary) energy γ now of the gyroscope signal is calculated using a batch of size N. Then, the threshold γ new may be updated iteratively as follows. γ new =(1 - p)γ old +p·γ now Here, 0 < p < 1 is the parameter of the first-order filter. The most recent M batches are kept in the history to determine the parameter p. Then, a new batch is added to the M batches, and at the same time, the first batch is forgotten to form a new history. Then, the old variance σ old and the new variance σ newIn its most general form, a monotonic mapping f:σ new / σ old →p can be applied.

[0033] In reality, the signal threshold is not theoretical. It becomes more reliable when replaced by a hysteresis-type threshold. FIG. 7 shows a graph 700 of hysteresis behavior presented along three axes, in one or more embodiments. A determination of H0 that the swing boom is moving is made only if the signal energy 704 meets (e.g., exceeds or falls below) a certain predetermined factor of the threshold 702, and a determination of H1 that the swing boom is stationary is made only if the signal energy 704 does not meet (e.g., does not exceed or falls below) the predetermined factor of the threshold 704. The factor β dynamic and β static is the upper and lower thresholds (β dynamic γ and β static γ) may be different from the lower threshold. Furthermore, if the signal remains below the threshold factor for a certain predetermined time, the signal energy is considered to be below the lower threshold. Graph 700 shows a portion 706 of the signal below threshold 702, and the swinging boom is determined to be stationary after the predetermined time. This behavior is intended to increase the system's sensitivity to certain magnitudes of motion while reducing the number of false alarms.

[0034] In one embodiment, in addition to comparing the signal energy to a hysteresis-type threshold, a zero-crossing rate Z for a batch of size N is used to handle situations where the signal energy is increasing due to increased noise rather than motion. ω is calculated as follows: TIFF0007783400000031.tif2159 Z ω Then, two thresholds T ZCR,low and T ZCR,high The signal is compared against Z ω >T ZCR,highIf so, it is considered dynamic, and Z ω <T ZCR,low These two decisions are combined to form the final decision H0 or H1 as the combination of both low energy (in a predetermined period of time) and zero crossings below a threshold.

[0035] By determining whether the swing boom is stationary (i.e., the swing angle is not changing) or moving (i.e., the swing angle is changing), the Kalman filter can calculate an estimated swing angle of the swing boom by either: 1) If it is determined that the swing boom is stationary (step 606), the received sensor data is corrected based on the observed swing angle, and an estimated swing angle is calculated based on the corrected sensor data; or 2) If it is determined that the swing boom is moving (step 608), the estimated swing angle is calculated based on the received sensor data. Calculation of the estimated swing angle based on determining whether the swing boom is stationary or moving is illustrated by the state machine 800 in FIG. 8. The Kalman filter is simply a one-dimensional observer for the bias of the difference in velocity between the vehicle body and the swing gyroscope about the Z axis (assuming the only error source is the relative bias random walk). TIFF0007783400000032.tif1256, TIFF0007783400000033.tif1430

[0036] The correction works according to a two-state state machine shown in Figure 8. In State #1, the angle of the swing joint is changing and the filter continuously integrates the gyroscope input. In State #2, the observed angle θ o is used to remove bias and correct the estimates. TIFF0007783400000034.tif926, TIFF0007783400000035.tif1630.

[0037] FIG. 8 illustrates a state machine 800 for determining the swing angle of a swing boom of a vehicle in one or more embodiments. The state machine 800 includes a state #1 corresponding to the swing boom moving relative to the vehicle body and a state #2 corresponding to the swing joint being stationary relative to the body. When the state machine 800 is in state #1 and the swing boom is determined to be stationary (i.e., zero motion detection occurs), the state machine 800 transitions from state #1 to state #2. When the state machine 800 is in state #2 and the swing boom is determined to be moving (i.e., motion detection occurs), the state machine 800 transitions from state #2 to state #1. In state #1, the state machine 800 calculates an estimated swing angle based on received sensor data. TIFF0007783400000036.tif83 is calculated. In state #2, the received sensor data is the observed swing angle θ o and the estimated swing angle is calculated based on the corrected sensor data. TIFF0007783400000037.tif83 is calculated.

[0038] In step 606 of Figure 6, if it is determined that the swing boom is stationary, the received sensor data is corrected based on the observed swing angle, and an estimated swing angle is calculated based on the corrected sensor data. As shown in Figure 4, the observed swing angle θ o may be received from the tilt estimation module 408, the rotation axis estimation module 412, and / or the error estimation module 414. The received sensor data may be used to estimate the observed swing angle θ o and the most recent estimated swing angle θ s Then, based on the corrected sensor data, a Kalman filter is used to calculate the estimated swing angle by, for example, integrating the difference in angular velocity between the swing boom and the vehicle body. TIFF0007783400000038.tif83 is calculated.

[0039] In one example, as shown in FIG. 5, when ZMD 512 determines that the swing joint is stationary, gate 516 is triggered and the most recent estimated swing angle θ s 518 is input to a 1D angular Kalman filter (KF) 510, which calculates the most recent estimated swing angle θ s 518 and the observed swing angle θ o and correcting the sensor data by comparing the sensor data with the estimated swing angle based on the corrected sensor data. When the ZMD 512 determines that the swing boom has transitioned from a moving state to a stationary state, the gate 514 is activated and calculates the most recent estimated swing angle θ s 518 Estimated Swing Angle Update with TIFF0007783400000040.tif83. In the context of the Kalman filter, the observed swing angle θ o The reliability of the gyroscope is usually low. TIFF0007783400000041.tif83 is calculated, but due to noise, it is necessary to observe the drive appropriately. Therefore, the observed swing angle θ o is the estimated swing angle The most recent estimated swing angle θ is used to calculate TIFF0007783400000042.tif83 s It is combined with 518.

[0040] In step 608 of Figure 6, if it is determined that the swing joint is operating, an estimated swing angle is calculated based on the received sensor data. TIFF0007783400000043.tif83 is calculated using a Kalman filter based on the received sensor data, for example, by integrating the difference in angular velocity between the swing boom and the vehicle body. In one embodiment, as shown in FIG. 5, when ZMD 512 determines that the swing joint is stationary, angle KF 510 is calculated as the estimated swing angle by, for example, integrating the difference between the angular velocity of the swing boom and the angular velocity of the swing output by summer 508. Calculate TIFF0007783400000044.tif83.

[0041] In some embodiments, the estimated swing angle TIFF0007783400000045.tif83 may exceed the vehicle swing limit and cause the Kalman filter to reset. For example, as shown in Figure 5, the estimated swing angle Angle KF 510 is reset when limit detector 520 determines that TIFF0007783400000046.tif83 exceeds the vehicle's swing limit. This limit is set by limit calibrator 522, for example, according to workflow 300 of FIG. 3. Resetting the Kalman filter means manually setting the value of the Kalman filter's state. When the swing limit is reached, the correct state is known, so the state can be updated directly.

[0042] In step 610 of Figure 6, the estimated swing angle is output. For example, the estimated swing angle can be output by displaying the estimated swing angle on a display device of a computer system, storing the estimated swing angle in memory or storage of a computer system, or transmitting the estimated swing angle to a remote computer system. As one example, the estimated swing angle is output to a user interface 900 shown in Figure 9. As another example, as shown in Figure 4, the estimated swing angle TIFF0007783400000047.tif83 is the observed swing angle θ o may be output by the kinematic angle estimation module 406 to the error estimation module 414 to generate:

[0043] FIG. 9 illustrates an exemplary user interface 900 according to one or more embodiments. The user interface 900 may be presented to a user, such as an operator of an excavator (or other suitable vehicle). As shown in FIG. 9, the user interface 900 visually equates the full range of swing limits to facilitate initialization by showing a layout 902 of the excavator's swing boom at zero attitude. The swing boom swing limits 904 are shown as ±40°. The user interface 900 may also show a working direction indicated by a user-defined line, facilitating the rotation and swing of the swing boom to align it along a certain direction.

[0044] The user interface 900 further displays a current estimated position 906 of the swing boom, determined based on the current estimated swing angle, and a current observed position 908 of the swing boom, determined based on the current observed swing angle. Various parameters 914 (e.g., Cartesian bucket position) may be updated according to the estimated swing angle. The current observed position 908 of the swing boom is displayed as an outline. The current observed position 908 indicates the position of the swing boom as perceived by the system. As shown in FIG. 9 , the current observed position 908 is very close to, but may not be identical to, the current estimated position 906 of the swing boom. This is because the estimated swing angle does not correspond to the observed swing angle, but the observed swing angle is used to correct the gyroscope bias according to the parameters of the Kalman filter. At initialization, the current observed position 908 and the current estimated position 906 may be very far apart. As the operator operates the vehicle, the current observed position 908 and the current estimated position 906 converge to a single point. A rating band 910 may be shown depicting the error between the current estimated position 906 and the current observed position 908. The rating band 910 may be color-coded (e.g., green, yellow, red) by rating, and the range for each rating may be user-defined. A circular variance indicator 912 may be shown, with the size of the indicator 912 indicating the variance and the resulting change in the current observed position 908. Thus, the larger the indicator 912, the greater the variance and the greater the uncertainty in the current observed position 908, and consequently, the greater the uncertainty in the current estimated position 906. The user interface 900 may also display an indicator informing the operator of the excavator's status. This status goes beyond indicators for normal and imperfect status and includes sub-states of the online state (e.g., latched or chasing). That is, the user interface 900 is designed to keep the operator informed of any degradation in accuracy that may occur while traveling. A past alternative would be an on-off indicator for low or high accuracy.

[0045] Estimating the rotation axis

[0046] During operation, the vehicle body is subject to slight vibrations as the boom swing operates, for example as a result of the operation of the boom, stick, or bucket pitch and tilt. Such vibrations are related to the observed swing angle θ o The rotation axis estimation algorithm uses the observed swing angle θ o 4 to determine the rotation axis estimation module 412 of FIG.

[0047] According to the above assumptions, at any given time and at a given swing angle, the swing IMU and the vehicle IMU can be considered as being attached to the same rigid body, but one rotating relative to the other. The basic idea is that when the rigid body is moving, even the measurements of a single entity sensed by both will have an observable difference in their outputs (proportional to the angle). The motion has many components perpendicular to the axis of rotation. However, the rigid body (i.e., the excavator's cabin) does not have a definite motion. That is, during operation, the excavator remains in a fixed position, and only its arm moves. The motion of the cabin and swing is parallel to the axis of rotation. Therefore, designed maneuvers are unreliable.

[0048] FIG. 10 illustrates a simplified model 1000 of an excavator according to one or more embodiments. The excavator model 1000 begins with a carbody link 1002 as the first link in the kinematic chain, followed by a swing link 1004 and terminating with a boom link 1006. The carbody link 1002 forms the chassis and track of the excavator with a rigid body connected to the ground via a massed spring-damper element 1008. Torque changes at the joints cause damped vibrations of the carbody. This becomes evident at the end of the hydraulic piston stroke (when the hydraulic valve closes and the mechanism abruptly stops), after which the situation resembles the free vibration of a car chassis after braking. This effect can be easily recognized (and felt) by a jab at the operator's control lever. This phenomenon can be modeled as a damped free vibration of a vibrating structure following an impulse load.

[0049] For illustrative purposes, FIG. 11 shows a graph 1100 of raw gyroscope signals recorded from an actual machine operation, according to one or more embodiments. Although the signal contains a lot of noise, there are clearly visible spots corresponding to vibrations, as shown in FIG. 11. In these segments, the signal-to-noise ratio (SNR) changes rapidly to provide sufficient observable information, while the remaining signal is very poor and nearly indistinguishable from noise. FIG. 12 shows a graph 1200 of gyroscope signals captured by two IMUs mounted on the same fixed platform, one rotating relative to the other, as implemented in a test rig according to one or more embodiments. FIG. 13 shows a model 1300 of the test rig according to one or more embodiments. The test rig model 1300 includes IMUs 1308 and 1310 positioned on a platform 1302, with IMU 1308 rotating relative to IMU 1310. The platform 1302 is supported at one end by a rod 1304 and at the other end is connected to the one end via a tilt joint and a spring 1306 .

[0050] The reason for the test setup is to verify that motion of the platform 1302 occurs only around a fixed axis of rotation. Similarity between the signals then confirms that manipulation of the arm actually causes the excavator body to oscillate around a fixed axis of rotation. It can then be concluded that the IMUs 1308, 1310 are providing information about this axis. However, while the non-rotating IMU 1310 records motion around its Y axis (the platform's axis of rotation), the rotating IMU 1308 sees motion around all of its axes (oscillations around the same axis of rotation expressed in its own local coordinate frame).

[0051] A simple and effective rotation axis estimation algorithm is implemented to identify a constant rotation axis from raw gyroscope measurements. For one batch of N measurements on TIFF0007783400000048.tif941, the rotation axis is given by: TIFF0007783400000049.tif1419 here TIFF0007783400000050.tif1747 Averaging works well even when the speed is sufficiently fast. However, the algorithm does not work when the speed is close to zero or crosses zero. The median operation is presented, and normalized samples are first shown in the spherical coordinate system. TIFF0007783400000051.tif1656 where TIFF0007783400000052.tif1833 The axis of rotation is then calculated as follows: TIFF0007783400000053.tif1936 where TIFF0007783400000054.tif53 and TIFF0007783400000055.tif63 indicates the median.

[0052] FIG. 14 illustrates a graph 1400 that is an example output of a rotation axis estimation algorithm based on damped oscillation, according to one or more embodiments. FIG. 15 illustrates a graph 1500 that shows the change in rotation axis as the swing IMU rotates, according to one or more embodiments. To generate graph 1500, the device is oscillated at gradually varying angles. This produces a clear signal that can accurately model damped oscillations, separating the oscillation portions from one another. FIG. 16 illustrates a plot 1600 of the signal used to calculate the rotation axis, according to one or more embodiments.

[0053] Rotating axis estimation algorithms assume that the rotating axis remains constant over some sufficiently long interval. Test equipment can easily generate these with carefully planned excitations, but in real machines the signal-to-noise ratio is low and vibrations overlap with chaotic noise variations in the rotating axis. There are many strategies for identifying vibrations over time.

[0054] There are many methods for identifying whether a signal is vibration-related and for calculating the amplitude and frequency of those vibrations. The focus here is not on the vibration itself, but on the oscillations that best represent the vehicle's motion. At all other times, the vehicle's motion is at the level of noise. Fourier analysis can, in principle, be used to estimate the components of a general signal. However, curve-fitting methods are more suitable for estimating the characteristics (e.g., amplitude, frequency, phase, damping) of a single sinusoidal component. Such methods seek to find the parameters of a function of the form: TIFF0007783400000056.tif648 formula (1) where α, λ, ω, β, and b are the amplitude, damping, frequency, phase, and offset, respectively. This function does not model chirp effects, and adding more harmonics may better describe the signal. Another way to specify the parameters (e.g., stiffness k, damping c, mass m) of a lumped mass-spring-damper physical system (or a model thereof) that generates vibration is as follows: TIFF0007783400000057.tif847 This equation represents a 1DOF system. A more realistic model would be a set of these. However, the goal here is not to model exactly, but to identify patterns of behavior. Regardless of the method used, it makes it easy to preprocess the signal and extract parts that are likely to allow further identification. The following preprocessing steps may be performed: 1) Store the most recent N samples in a buffer. Display by TIFF0007783400000058.tif531. 2) Detrend it(s). 3) Normalize it using max(s) and min(s). 4) Find the peak. 5) Search for the largest peak. 6) Remove samples before this peak. 7) Start with the first sample and work your way towards the end of the buffer until you find a larger peak. 8) At the end, remove the remaining sample. Reindex TIFF0007783400000059.tif532. 9) Find the zero crossing. 10) Estimate the period p of the oscillation as twice the average distance between two consecutive zero crossings. 11) Using an optimizer, apply s to the function f defined in equation (1). The initial values ​​of α, λ, ω, β, and b are |max(s)-min(s)|, 0.1, 2π / p, and TIFF0007783400000060.tif42 (average of buffer), etc. Figure 17 illustrates a graph 1700 of an example of applying s to a function f, in accordance with one or more embodiments.

[0055] Given an estimate of the axis of rotation of the vehicle body and swing boom, the observed swing angle is calculated as follows: TIFF0007783400000061.tif1473

[0056] The rotation axis estimation algorithm described above is very sensitive to noise. Experiments have shown that angles below a certain value (e.g., 25°) cannot be handled by this method because the gyroscope rate component on one axis becomes as small as the noise itself. Therefore, in some embodiments, other methods for determining the angle observations may be implemented.

[0057] error estimation

[0058] 18 illustrates a schematic diagram of an error estimation module 1800 according to one or more embodiments. In one example, the error estimation module 1800 is the error estimation module 414 of FIG.

[0059] The error estimation is performed by transfer alignment. Transfer alignment is a set of methods for aligning two sensors (e.g., IMUs), one of which is a master sensor and the other a slave sensor. Transfer alignment works by comparing, for example, angular rates, velocities, accelerations, etc., measured by the master-slave unit. The error estimation by transfer alignment is performed by comparing the observed swing angle θ ois applied to calculate the observed swing angle, where 1) the angular error is large (e.g., up to 40°) and is not typically considered a misalignment, and 2) the vehicle is unable to perform any designed maneuvers.

[0060] The body sensor located on the body is the master and the swing sensor located on the swing boom is the slave. The alignment error is defined in terms of pitch, roll and yaw of the swing boom relative to the body. TIFF0007783400000062.tif712 is expanded by the following differential equation: TIFF0007783400000063.tif1040 where: TIFF0007783400000064.tif2174 and TIFF0007783400000065.tif1012 is a skew matrix composed of the gyroscopic velocity of the boom swing relative to the vehicle body. TIFF0007783400000066.tif1076

[0061] Transfer alignment assumes small angle misalignment. TIFF0007783400000067.tif1052 (Formula 2) where I is the identity matrix, TIFF0007783400000068.tif815 is a skew matrix whose off-diagonal elements are the orientation errors. TIFF0007783400000069.tif1749

[0062] The error can be shown to evolve (propagate) according to the following differential equation: TIFF0007783400000070.tif1085 where x denotes the vector cross product operation, TIFF0007783400000071.tif833 is the error vector, and the gyroscope error is given as: TIFF0007783400000072.tif948TIFF0007783400000073.tif944 (Formula 3)

[0063] Similar results are extracted using accelerometer readings. TIFF0007783400000074.tif1144 where: TIFF0007783400000075.tif910 is the linear velocity vector of the vehicle (i.e., truck), TIFF0007783400000076.tif1010 is the specific force (accelerometer output) measured by the IMU placed on the swing boom, TIFF0007783400000077.tif83 is the gravity vector inside the vehicle frame. It can be shown that the following differential equation governs the propagation: TIFF0007783400000078.tif1064 where: TIFF0007783400000079.tif1039 shows the error in speed calculation. TIFF0007783400000080.tif910 is the vehicle acceleration measured by an IMU placed on the vehicle's body, TIFF0007783400000081.tif1042 is the difference between the true acceleration and the measured acceleration.

[0064] This formulation leads to the implementation of a Kalman filter. For this purpose, the following assumptions are made: 1) The error in the vehicle velocity is assumed to be zero (i.e., TIFF0007783400000082.tif1022), 2) the swing velocity error is modeled as Gaussian white noise (i.e., TIFF0007783400000083.tif949), 3) The swing accelerometer error is modeled as Gaussian white noise (i.e., TIFF0007783400000084.tif947).

[0065] The state space model used by the Kalman filter is as follows: TIFF0007783400000085.tif845TIFF0007783400000086.tif1459 where: TIFF0007783400000087.tif1046 is the state vector, TIFF0007783400000088.tif1054 is the noise component of the gyroscope and accelerometer. TIFF0007783400000089.tif1342TIFF0007783400000090.tif1539TIFF0007783400000091.tif1939 is.

[0066] The above can be discretized as follows: TIFF0007783400000092.tif861TIFF0007783400000093.tif852

[0067] The Kalman filter works based on observations of state (rate and rotation errors). z k =H k x k +v k where v k is a measure of the noise. Based on the available observations, H kcan take a variety of forms, including speed matching and speed ratio matching.

[0068] In speed matching, Direct observations of TIFF0007783400000094.tif1035 are given as follows: TIFF0007783400000095.tif858 where: TIFF0007783400000096.tif82 is an estimate of the vector from the body IMU to the swing IMU. This is a function of the estimate θ and fixed machine parameters (e.g., the distance from the body IMU to the swing joint, and the distance from the swing joint to the swing IMU). Thus, TIFF0007783400000097.tif1441 is.

[0069] In speed ratio matching, the observed difference between the vehicle (body) speeds is calculated by the body IMU and estimated by the swing IMU, so TIFF0007783400000098.tif1050 This becomes: Using equations 2 and 3, this can be shown as follows: TIFF0007783400000099.tif1079 If we model TIFF0007783400000100.tif1026 as Gaussian white noise, it becomes TIFF0007783400000101.tif1026. In this case, v k also includes the flexure. In the context of an excavator, the flexure is the unknown, unmodeled uncertainty due to the mechanical slop of the swing joint.

[0070] Transfer alignment can also be performed based on the correspondence between attitude and acceleration. In the application of error estimation of the swing angle of an excavator, a number of simplifications can be achieved. First, the excavator rarely moves, and when it does, the movement is not when the swing boom is moving, so speed ratio correspondence is preferred. Second, the Kalman filter can be used to estimate the error when the swing boom is not swinging and ω z,swing It is executed at a time when it can be assumed that is approximately zero.

[0071] If an error estimate is available at this time, the current estimated swing angle θ is added to the error estimate in the Z axis, so the kinematic filter observation θ o is generated. TIFF0007783400000102.tif821

[0072] 19 shows a graph 1900 illustrating the results of transfer alignment in one or more embodiments. Plot 1902 shows the swing joint angle, plot 1904 shows the swing angle error, plot 1906 shows the corrected swing joint angle, and plot 1908 shows the corrected swing angle error. Graph 1900 was generated by rotating the slave (swing) IMU by 20°. As can be seen from graph 1900, the transfer alignment filter converges to the true angle. The subsequent oscillations cause the state to fluctuate around the desired value.

[0073] The signal quality is not an ideal setup. Furthermore, the vibrations do not occur in well-separated chunks. Transfer alignment is very robust to noise and is not required given a suitable signal (e.g., the signal considered to calculate the rotation axis), but care must be taken. For example, it is assumed that transfer alignment results for signals with very small amplitudes or very short signals are not sufficient and should be discarded. Even when these assumptions are applied properly, the presence of outliers is inevitable. Also, the distribution of errors can lead to over-estimated values.

[0074] It should be noted that the fundamental reason for the correction is to prevent the gyroscope estimate from drifting, so strategies may be devised to remove outliers and dynamically modify the variance in which the correction is applied to produce smoother estimates.

[0075] Strength (Robustness)

[0076] This section specifically addresses methods to facilitate smoother, more stable, and more robust system implementation.

[0077] Naturally, the observed swing angle θ o There is uncertainty in the RMS (root mean square) value used to correct the kinematic angle filter. This is reflected in TIFF0007783400000103.tif813. The RMS value determines how quickly the observations converge. In one embodiment, the convergence rate can be determined by creating an envelope around the observations. The actual observed signal is: TIFF0007783400000104.tif520 where TIFF0007783400000105.tif52 is the width of the uncertainty envelope. TIFF0007783400000106.tif52 is 2°.

[0078] The initial amplitude and duration of the oscillation are effective strategies to avoid poor quality and potentially dangerous error estimation. The duration has been found to be less influential than the amplitude, because the rapidly decaying tail of the oscillation is at the level of the noise itself. However, the oscillation must be long enough to include at least the first few periods. The minimum values ​​of these parameters can be obtained by analyzing the raw signal.

[0079] As explained for estimating the axis of rotation, extracting signal segments that fit a model of damped oscillations ensures the success of algorithms operating on that signal. While it is easy to attach a model to a signal stream and try to determine whether it is suitable, designing an algorithm that is effective on segments that are sufficiently similar to the model is not trivial. Furthermore, the inherent nature of good fitting measures such as MSE can lead to overconservativeness, leading to unnecessary discards.

[0080] One important feature of the system is the separation of states into moving and resting. Initially, the system does not know the true value of the swing angle. In this case, it pursues the true value of the swing angle by applying an error estimate (no matter how large). The system calculates statistics (e.g., mean and variance) of the input error estimate. When the mean becomes zero and the variance becomes small enough, the estimate has a high probability of adhering to the true swing angle. This requires vibrating the vehicle body for a sufficient time by moving the arm. An alternative method is for the excavator operator to hit the swing limit. Which method is faster depends on the window size selected for statistical processing. The advantage of transitioning to this state is that many limits on the gate observation can be provided based on the error estimate. The rationale is that, considering the characteristics of the gyroscope signal and assuming the joint mechanism operates sufficiently close to a single-axis rotary joint, once the true angle is identical or close to the true angle, the main factor causing deviation is the slow drift due to gyro random walk. In particular, in this mode, smooth estimation can be achieved by the following safety measures:

[0081] In most cases, the observed swing angle θ o is the current estimated swing angle It fluctuates around TIFF0007783400000107.tif63. Estimated swing angle TIFF0007783400000108.tif63 is the observed swing angle θo The closer it is to the observed swing angle θ o is the estimated swing angle The probability that the current estimate is correct is greater. If the range is within TIFF0007783400000110.tif526, the correction variance is adaptively changed. TIFF0007783400000111.tif1265 where m is the amplitude. Half the width of the uncertainty envelope is equal to 2σ of the normal distribution.

[0082] Statistical outlier analysis can facilitate the rejection of erroneous corrections when the distribution is known. TIFF0007783400000112.tif56 and distribution If TIFF0007783400000113.tif96 is small enough, we can reasonably assume that the normal PDF (probability density function) reflects the true error distribution, so we can use a Z-test to reject outliers. More formally, For TIFF0007783400000114.tif816, the estimated error TIFF0007783400000115.tif65 will be rejected, except as follows: TIFF0007783400000116.tif1129

[0083] Another, even simpler strategy is to constrain the magnitude of the error to a certain range: if the range chosen is identical to the width of the uncertainty range, this is effectively equivalent to using the sign of the error to center the estimate.

[0084] A similar strategy is to use an error deadband with an uncertainty envelope size: as long as the angle estimate is within the deadband, no correction is applied to the angle estimation filter.

[0085] The system operates on the principle of continuous excitation. Thus, a given error estimate can be considered stale after it has been applied for a certain period of time. This prevents an off-target error estimate from leading to an excessively long-term estimate, especially when the operator stops operation and a particular correction happens to be the last. Similarly, when the swing angle changes, the current correction (estimated before the operation) must stop and wait for the next observation.

[0086] After an initialization phase (i.e., a phase where estimates may fluctuate even when no physical swing is occurring), the magnitude of the error estimate must be commensurate with the amount of swing, i.e., predicted as follows: TIFF0007783400000117.tif1481 where t0 and t1 represent two consecutive times at which the error estimates arrive. TIFF0007783400000118.tif1014 is an outlier, Assume the image is TIFF0007783400000119.tif1023. The integral is TIFF0007783400000120.tif912. To handle this case, the system Increase the correction variance according to TIFF0007783400000121.tif1048, where: TIFF0007783400000122.tif1364 is.

[0087] Slope Estimation

[0088] When the vehicle is positioned at a sufficient incline, the observed swing angle θ ocan be calculated in a simpler way. o can be calculated based on the incline estimation by incline estimation module 408 of FIG. 4 and the incline determination by incline estimation module 410. In one embodiment, the condition for a vehicle to be considered at a sufficient incline is that the minimum chassis roll and pitch angles are at least 6° or greater (i.e., a 12% incline threshold) and remain constant during operation. This is greater than the desired incline in most applications, and the assumption of constant chassis incline can easily be violated on steep inclines. Furthermore, the rotation of the cabin relative to the track must be known. Since the chassis is not typically mounted, this requires a calibration phase each time the incline changes, which is only performed if an existing GPS-based (or equivalent optical) guidance system and / or automatic control system exists to provide the cabin's heading. However, if the above assumption is valid, the advantages are clear.

[0089] First, the rotation matrix TIFF0007783400000123.tif1012 is expressed so that the axis of rotation of the swing is made explicit, using the matrix form of Rodrigues' formula. TIFF0007783400000124.tif1379, where: TIFF0007783400000125.tif619 is the swing joint axis, θ is the rotation angle of the swing joint, TIFF0007783400000126.tif57 is a skew matrix. TIFF0007783400000127.tif1539

[0090] The gravity vector TIFF0007783400000128.tif926, where ( TIFF0007783400000129.tif824), then the output of the accelerometer (when stationary) will be: TIFF0007783400000130.tif828 The above notation uses the vertical axis ( This may be extended to the case of a general orientation sensor that rotates around a point (or axis) (TIFF0007783400000131.tif928), where the rotation angle is uniquely in the range [-π,π). Similar logic is used to obtain identical results for an arbitrary rotation (swing) axis and a fixed orientation (gravity).

[0091] The above formula can be expanded as follows: TIFF0007783400000132.tif3284. where the left hand side is the accelerometer output. To simplify the above notation to special (and more tractable) cases, a fundamental change is made: TIFF0007783400000133.tif53 TIFF0007783400000134.tif920. In other words, such coordinate changes transform the 3D space into a rotating plane (whose normal vector is TIFF0007783400000135.tif53). Such a transformation is TIFF0007783400000136.tif53 TIFF0007783400000137.tif516 Here, TIFF0007783400000138.tif55 is the appropriate linear operator (3x3 matrix), which results in: TIFF0007783400000139.tif1449 The above equation can be succinctly expressed as a complex number. TIFF0007783400000140.tif649 this is, TIFF0007783400000141.tif1159 where θ relates the phase of the accelerometer data to the initial phase before rotation (i.e., the amount of rotation of the cabin around its own axis). Although a useful formula to account for the unobservability of swing rotation in the general case, operation using raw accelerometer data is difficult (due to noise and linear acceleration), and the above correction is only applicable at specific times when the estimated gravity is stable. The effort for gravity estimation can instead be spent on estimating the pitch and roll of the swing IMU, exploiting the behavior of the gyroscope. Assuming the pitch and roll of that IMU are available, the following formula TIFF0007783400000142.tif1467 gives the direction in tilt, i.e., the swing angle. Subtracting the cabin rotation from the swing angle gives the observed swing angle θ0. The tilt is given by: TIFF0007783400000143.tif1466

[0092] However, for the (swing) angular direction to be observable, the magnitude of either pitch or roll, or both, must exceed a threshold. The above measurement returns the same value regardless of the magnitude of pitch and roll when they are equal. As the rotation axis approaches vertical, observability disappears and the variance becomes infinite.

[0093] Using the Swing Boom Sensor

[0094] In some embodiments, the vehicle may already have existing sensors (e.g., IMUs) installed. In these embodiments, the kinematic angle estimation module receives sensor data from the existing sensors, thereby reducing costs. The error estimation formula remains the same. However, complexity increases and the quality of the estimation may be reduced. This is because the existing sensors on the swing boom measure two motions: the motion of the vehicle body and the motion of the swing boom relative to the vehicle body. Therefore, estimating the rotation matrix of the swing relative to the vehicle body is no longer sufficient; a pitch component must be included. The challenge is to extract the common motion where the existing sensors experience the same pitch motion. The remaining pitch component is added to the error.

[0095] The first consideration is the calculation of the inclination (pitch and roll) of the system (particularly the body, swing link, and boom) with respect to the parts of interest. Attitude estimation may be performed using a unit quaternion representation of rotations and employing a Kalman-type filter as the observer. Other representations (e.g., DCM (direction cosine matrix) and Euler angles) and other observers (e.g., interpolation filters and nonlinear observers) may also be employed.

[0096] q=(q w ,q x ,q y ,q z ) encodes the attitude of the IMU, such as the cabin, swing, or boom, and its time evolution is TIFF0007783400000144.tif2975 where: TIFF0007783400000145.tif923, TIFF0007783400000146.tif713, TIFF0007783400000147.tif54 is the output of the gyroscope TIFF0007783400000148.tif610, TIFF0007783400000149.tif830, and b=(b x b y b z ) is the gyro bias, TIFF0007783400000150.tif55 is a quaternion multiplication and TIFF0007783400000151.tif1949TIFF0007783400000152.tif1949 and the equation in the most recent equation TIFF0007783400000153.tif668 The filter status is TIFF0007783400000154.tif525, and the filter estimates the bias. Zeroth-order forward integration can be used to derive the discrete-time state expansion of the extended Kalman filter. TIFF0007783400000155.tif1374, TIFF0007783400000156.tif618 The state transition matrix and noise matrix are TIFF0007783400000157.tif1267 and TIFF0007783400000158.tif1545 is given by

[0097] Note that yaw angles are not of interest, so no yaw angle observations are provided. To correct for pitch and roll, the gravity vector expressed in the IMU local frame is Let us assume for the moment that estimates for TIFF0007783400000159.tif42 are available. Innovation can be calculated as follows: TIFF0007783400000160.tif533 where: TIFF0007783400000161.tif59 is the rotation matrix representation as a function of the quaternion orientation. Thus, the observation matrix is TIFF0007783400000162.tif1481 is given by

[0098] There are two main methods for estimating the gravity vector: "static" and "dynamic."

[0099] Static: The filter is calibrated only during the time period when the accelerometer output is equal to gravity in the IMU local frame. This is the case when the variance of the accelerometer output is small enough and its magnitude is close enough to g. Then, using an energy-based strategy, the stationary hypothesis (H1) can be determined when both of the following relationships are true: TIFF0007783400000163.tif1246TIFF0007783400000164.tif1250 where: TIFF0007783400000165.tif96 is the scale factor for this special case. A disadvantage of this method is that if the machine does not stabilize frequently and for a sufficient period, the corrections may not be made frequently enough. To improve this limitation, γ can be increased, along with the error variance of the corrections. The main advantage of this method, in addition to its simplicity, is that it is independent of any kinematic information possessed by the machine.

[0100] Dynamic: Compensate for linear acceleration to correct the filter at all times. This method is suitable for highly dynamic and / or noisy environments. The compensation may be performed by optimally varying the observation covariance matrix. This may be performed using a robust Kalman filter, especially if acceleration can be calculated with sufficient accuracy.

[0101] In general, p imuIf (t) denotes the position of the IMU in global space, then p base When considered as the end point of a serial mechanism with a base located at (t) (e.g., the center of the body), P imu (t)=P base (t)+K DH (θ) Here, K DH (θ) denotes the kinematic mapping calculated using the Denavit-Hartenberg convention, where θ is the joint angle vector. Similarly, TIFF0007783400000166.tif646 where, TIFF0007783400000167.tif831 is the Jacobian of the serial link mechanism, TIFF0007783400000168.tif63 is the rotational velocity of the corresponding joint. Assuming the vehicle remains stationary during motion (a reasonable assumption barring occasional slippage), the linear acceleration sensed by the sensor is a representation of the acceleration that moves the IMU in space within the local sensor frame. TIFF0007783400000169.tif1049

[0102] Therefore, the local gravity to correct the attitude filter is The above applies to the body, swing and boom IMUs, so θ is estimated as (θ body ), (θ body ,θ swing ) and (θ body ,θ swing ,φ boom ) and similarly TIFF0007783400000171.tif63 is (ω z,body ), (ω z,body ,ω z,swing -ω z,body ) and (ωz,body ,ω z,swing -ω z,body ,ω y,boom -ω y,swing ) becomes.

[0103] The kinematic mapping and its Jacobian are also functions of certain lengths that are typically measured as part of the system calibration: the horizontal distance between the base and the body IMU, the horizontal distance between the base and the swing joint, the distance between the swing joint and the swing IMU, the distance between the swing joint and the boom joint, and the distance between the boom joint and the boom IMU.

[0104] Finally, schemes based on linear acceleration estimation (as in this system) are generally more robust if some adaptive variance is applied to cover uncertain cases, especially when the vehicle skids or starts to move and velocity information is unavailable.

[0105] The systems, devices, and methods described herein may be implemented using digital circuitry or one or more computers using known computer processors, memory units, storage devices, computer software, and other elements. Typically, a computer includes a processor for executing instructions and one or more memories for storing instructions and data. A computer may include or be coupled to one or more mass storage devices, such as one or more magnetic disks, internal hard disks, removable disks, magneto-optical disks, and optical disks.

[0106] The systems, apparatus, and methods described herein may be implemented using computers operating in a client-server relationship. Typically, in such a system, the client computers are remote from the server computer and interact over a network. The client-server relationship may be defined and controlled by computer programs running on each of the client and server computers.

[0107] The systems, devices, and methods described herein may be implemented within a network-based cloud computing system. In such a network-based cloud computing system, a server or other processor connected to a network communicates with one or more client computers over the network. For example, a client computer may communicate with the server through a network browser application resident and running on the client computer. The client computer may store data on the server and access the data over the network. The client computer may send a request for data or an online service to the server over the network. The server may perform the requested service and provide the data to the client computer. The server may send data adapted to cause the client computer to perform a particular function, such as to perform a calculation or to display particular data on a screen. For example, the server may send a request to the client computer adapted to cause the client computer to perform one or more steps or functions in the methods and workflows described herein (including one or more steps and functions in Figures 4-6 and 18). Particular steps or functions of the methods and workflows described herein (including one or more steps or functions in Figures 4-6 and 18) may be performed by a server or other processor within the network-based cloud computing system. Certain steps or functions of the methods and workflows described herein (including one or more steps in Figures 4-6 and 18) may be performed by a client computer within a network-based cloud computing system. The steps or functions of the methods and workflows described herein (including one or more steps in Figures 4-6 and 18) may be performed by a server and / or a client computer, in any combination, within a network-based cloud computing system.

[0108] The systems, devices, and methods described herein may be implemented using a computer program product tangibly embodied in an information medium (e.g., a non-transitory machine-readable storage device) for execution by a programmable processor, and the method and workflow steps described herein (including one or more steps or functions in Figures 4-6 and 18) may be implemented using one or more computer programs executable by such a processor. A computer program is a sequence of computer program instructions that can be used, directly or indirectly, in a computer to perform a particular activity or bring about a particular result. Computer programs can be written in any type of programming language (including compiled or interpreted languages) and can be deployed in any form (as a stand-alone program or as a module, component, subroutine, or other suitable unit for use in a computing environment).

[0109] FIG. 20 illustrates a high-level block diagram of an example computer 2002 that can be used to implement the systems, apparatus, and methods described herein. The computer 2002 includes a processor 2004 operatively coupled to a data storage device 2012 and a memory 2010. The processor 2004 executes computer program instructions that define the operation of the computer 2002 to control its overall operation. The computer program instructions may be stored in the data storage device 2012 or other computer-readable medium and loaded into the memory 2010 when execution of the computer program instructions is desired. Thus, the steps or functions of the methods and workflows of FIGS. 4-6 and 18 may be defined by computer program instructions stored in the memory 2010 and / or the data storage device 2012 and controlled by the processor 2004 executing the computer program instructions. For example, the computer program instructions may be implemented as computer-executable code programmed by one skilled in the art to perform the steps or functions of the methods and workflows in FIGS. 4-6 and 18. 4-6 and 18. The computer 2002 may include one or more network interfaces 2006 for communicating with other devices over a network. The computer 2002 may also include one or more input / output devices 2008 (e.g., a display, a keyboard, a mouse, speakers, buttons, etc.) that allow user interaction with the computer 2002.

[0110] Processor 2004 may include both general purpose and special purpose microprocessors and may be the sole processor or one of multiple processors in computer 2002. Processor 2004 may include, for example, one or more central processing units (CPUs). Processor 2004, data storage device 2012, and / or memory 2010 may include, be supplemented by, or be incorporated in, one or more application specific integrated circuits (ASICs), and / or one or more field programmable gate arrays (FPGAs).

[0111] The data storage device 2012 and the memory 2010 each comprise a tangible, non-transitory computer-readable storage medium. The data storage device 2012 and the memory 2010 each may include high-speed random access memory such as dynamic random access memory (DRAM), static random access memory (SRAM), double data rate synchronous dynamic random access memory (DDR RAM), or other random access solid-state storage devices, or may include non-volatile storage devices such as one or more magnetic disk storage devices, such as internal hard disks and removable disks, magneto-optical disk storage devices, optical disk storage devices, flash memory devices, semiconductor memory devices, such as erasable programmable read-only memories (EPROMs), electrically erasable programmable read-only memories (EEPROMs), compact disc read-only memories (CD-ROMs), digital versatile disc read-only memories (DVD-ROMs), or other non-volatile solid-state storage devices.

[0112] The input / output devices 2008 may include peripheral devices such as a printer, scanner, display screen, etc. For example, the input / output devices 2008 may include a display device such as a cathode ray tube (CRT) or liquid crystal display (LCD) monitor for displaying information to a user, a pointing device such as a mouse or trackball for providing input to the computer 2002 by the user, and a keyboard.

[0113] Some or all of the systems and devices described herein may be implemented using one or more computers, such as computer 2002.

[0114] Those skilled in the art will appreciate that an actual computer or computer system implementation may have other structures and elements, and that FIG. 20 is a high-level representation to illustrate some elements of such a computer.

[0115] The foregoing "Detailed Description" is to be understood in all respects as illustrative and exemplary, and not restrictive, and the scope of the invention disclosed herein should be determined not from the above "Detailed Description" but from the claims, which are to be interpreted in accordance with the full breadth permitted by patent law. It is to be understood that the embodiments shown and described herein are merely illustrative of the principles of the present invention, and that various modifications may be made by those skilled in the art without departing from the scope and spirit of the invention. Various other feature combinations may be implemented by those skilled in the art without departing from the scope and spirit of the invention.

Claims

1. 1. A computer-implemented method comprising: receiving sensor data from sensors located on a swing boom and a body of the vehicle; determining whether the swing boom is stationary or moving relative to the vehicle body based on the sensor data; When it is determined that the swing boom is stationary, correcting the received sensor data based on an observed swing angle and calculating an estimated swing angle based on the corrected sensor data; calculating the estimated swing angle based on the received sensor data when it is determined that the swing boom is operating; outputting the estimated swing angle; Equipped with The step of correcting the received sensor data based on an observed swing angle and calculating an estimated swing angle based on the corrected sensor data includes: comparing the observed swing angle with a most recent estimated swing angle; and removing bias from the sensor data based on the comparison.

2. The step of determining whether the swing boom is stationary or moving relative to the vehicle body based on the sensor data includes: calculating the energy of the signal received from the sensor; comparing the calculated energy to one or more thresholds; determining whether the swing boom is stationary or moving based on the comparison; 2. The computer-implemented method of claim 1, comprising:

3. determining that the estimated swing angle exceeds a swing limit of the vehicle; resetting a Kalman filter used to calculate the estimated swing angle; The computer-implemented method of claim 1 further comprising:

4. determining an axis of rotation of the sensor; calculating the observed swing angle based on the determined rotation axis; The computer-implemented method of claim 1 further comprising:

5. determining a swing angle error by transfer alignment; calculating the observed swing angle based on the swing angle error; The computer-implemented method of claim 1 further comprising:

6. and when it is determined that the vehicle is positioned on an incline that satisfies an incline threshold, calculating the observed swing angle based on a roll and a pitch of the vehicle.

10. The computer-implemented method of claim 1.

7. The sensor is an IMU (inertial measurement unit), 10. The computer-implemented method of claim 1.

8. the vehicle is an excavator; 10. The computer-implemented method of claim 1.

9. a memory for storing computer program instructions; at least one processor configured to process the computer program instructions; The computer program instructions may direct the at least one processor to: receiving sensor data from sensors located on a swing boom and a body of the vehicle; determining whether the swing boom is stationary or moving relative to the vehicle body based on the sensor data; When it is determined that the swing boom is stationary, correcting the received sensor data based on an observed swing angle and calculating an estimated swing angle based on the corrected sensor data; calculating the estimated swing angle based on the received sensor data when it is determined that the swing boom is operating; outputting the estimated swing angle; configured to cause the device to perform operations comprising: The step of correcting the received sensor data based on an observed swing angle and calculating an estimated swing angle based on the corrected sensor data includes: comparing the observed swing angle with a most recent estimated swing angle; removing bias from the sensor data based on the comparison; A system comprising:

10. The step of determining whether the swing boom is stationary or moving relative to the vehicle body based on the sensor data includes: calculating the energy of the signal received from the sensor; comparing the calculated energy to one or more thresholds; determining whether the swing boom is stationary or moving based on the comparison; The system of claim 9, comprising:

11. The operation is determining that the estimated swing angle exceeds a swing limit of the vehicle; resetting a Kalman filter used to calculate the estimated swing angle; The system of claim 9 further comprising:

12. The operation is determining an axis of rotation of the sensor; calculating the observed swing angle based on the determined rotation axis; The system of claim 9 further comprising:

13. A non-transitory computer-readable medium storing computer program instructions, comprising: The computer program instructions, when executed by a processor, cause the processor to: receiving sensor data from sensors located on a swing boom and a body of the vehicle; determining whether the swing boom is stationary or moving relative to the vehicle body based on the sensor data; When it is determined that the swing boom is stationary, correcting the received sensor data based on an observed swing angle and calculating an estimated swing angle based on the corrected sensor data; calculating the estimated swing angle based on the received sensor data when it is determined that the swing boom is operating; outputting the estimated swing angle; performing an operation comprising: The step of correcting the received sensor data based on an observed swing angle and calculating an estimated swing angle based on the corrected sensor data includes: comparing the observed swing angle with a most recent estimated swing angle; removing bias from the sensor data based on the comparison; 1. A non-transitory computer-readable medium comprising:

14. The operation is determining a swing angle error by transfer alignment; calculating the observed swing angle based on the swing angle error; 14. The non-transitory computer-readable medium of claim 13, further comprising:

15. The operation is and when it is determined that the vehicle is positioned on an incline that satisfies an incline threshold, calculating the observed swing angle based on a roll and a pitch of the vehicle.

14. The non-transitory computer-readable medium of claim 13.

16. The sensor is an IMU (inertial measurement unit), 14. The non-transitory computer-readable medium of claim 13.

17. the vehicle is an excavator; 14. The non-transitory computer-readable medium of claim 13.

18. An excavator, a first sensor disposed on a swing boom of the excavator; a second sensor disposed on the body of the excavator; a memory for storing computer program instructions; at least one processor configured to process the computer program instructions; The computer program instructions may direct the at least one processor to: receiving sensor data from the first sensor and the second sensor; determining whether the swing boom is stationary or moving relative to the vehicle body based on the sensor data; When it is determined that the swing boom is stationary, correcting the received sensor data based on an observed swing angle and calculating an estimated swing angle based on the corrected sensor data; calculating the estimated swing angle based on the received sensor data when it is determined that the swing boom is operating; outputting the estimated swing angle; configured to cause the device to perform operations comprising: The step of correcting the received sensor data based on an observed swing angle and calculating an estimated swing angle based on the corrected sensor data includes: comparing the observed swing angle with a most recent estimated swing angle; and removing bias from the sensor data based on the comparison.

Images