UNITS

CROSS REFERENCE

[0001] The present application claims the benefit and priority of U.S. Non-Provisional Application No. 15/887,717, filed on February 2, 2018, entitled,“RELATIVE ANGLE ESTIMATION USING INERTIAL MEASUREMENT UNITS,” the entire contents of which are incorporated herein by reference in their entirety for all purposes.

FIELD OF THE INVENTION

[0002] Embodiments described herein relate generally to estimating relative angles between coupled rigid bodies using inertial measurement units (IMUs).

BACKGROUND

[0003] IMUs are sensors that typically use acceleromters and gyroscopes to measure linear and angular motion of an object . IMUs are commonly used in a number of applications where information about object motion is desired. Examples include vehicles, aircraft, and satellites. IMUs are also commonly used to measure motion of different parts of a larger object.

[0004] Improved methods of determining relative angles between coupled rigid bodies using IMUs are desired.

SUMMARY

[0005] Embodiments described herein provide improved methods for determining relative angles and/or component relative angles sin (a) cos (a) between two or more coupled rigid bodies. The relative angles are determined using information from IMUs mounted on the rigid bodies. [0006] In accordance with an embodiment, a method for determining component relative angles between separate but coupled rigid bodies includes obtaining a gravitational acceleration vector and an angular velocity of a previous rigid body from observations of an IMU mounted on the previous rigid body and obtaining a gravitational acceleration vector and an angular velocity of a rigid body from observations of an IMU mounted on the rigid body. A first component relative angle and a second component relative angle between the previous rigid body and the rigid body are estimated based at least in part on the gravitational acceleration vector and the angular velocity of the previous rigid body and the gravitational acceleration vector and the angular velocity of the rigid body.

[0007] In an embodiment, the first component relative angle and second component relative angle between the previous rigid body and the rigid body are determined without using an absolute angle of the previous rigid body or an absolute angle of the rigid body.

[0008] In another embodiment, the first component relative angle is a sine component angle and the second component relative angle is a cosine component angle.

[0009] In another embodiment, the method also includes using the sine component angle and the cosine component angle to determine a relative angle between the previous rigid body and the rigid body.

[0010] In another embodiment, estimating the first component relative angle and the second component relative angles includes determining first and second observed component relative angles of the rigid body, determining a rotation matrix from the previous rigid body to the rigid body, and determining at least one covariance matrix.

[0011] In yet another embodiment, the first and second observed component relative angles of the rigid body are determined using the gravitational acceleration vector of the previous rigid body and the gravitational acceleration vector of the rigid body.

[0012] In accordance with another embodiment, a method for estimating component relative angles between coupled rigid bodies each having an IMU disposed thereon includes receiving a gravitational acceleration vector of a previous rigid body determined from observations of an IMU mounted on the previous rigid body, receiving an angular velocity of the previous rigid body from observations of the IMU mounted on the previous rigid body, receiving a gravitational acceleration vector of a rigid body determined from observations of an IMU mounted on the rigid body, and receiving an angular velocity of the rigid body from observations of the IMU mounted on the rigid body. First and second component relative angles between the previous rigid body and the rigid body are estimated based at least in part on the gravitational acceleration vector and the angular velocity of the previous rigid body and the gravitational acceleration vector and the angular velocity- of the rigid body. The method also includes receiving a gravitational acceleration vector and an angular velocity of a next rigid body determined from observation of an IMU mounted on the next rigid body. First and second component relative angles between the rigid body and the next rigid body are estimated based at least in part on the gravitational acceleration vector and the angular velocity' of the rigid body and the gravitational acceleration vector and the angular velocity of the next rigid body.

[0013] In an embodiment, the first and second component relative angles between the rigid body and the next rigid body are determined without using an absolute angle of the rigid body or an absolute angle of the next rigid body.

[0014] In another embodiment, the first and second component relative angles between the rigid body and the next rigid body are determined without using the gravitational acceleration vector of the previous rigid body.

[0015] In accordance with another embodiment, a method for determining component relative angles between a series of separate but coupled rigid bodies, including a previous rigid body and a rigid body, comprises receiving a gravitational acceleration vector of the previous rigid body' and a gravitational acceleration vector of the rigid body, and calculating first and second observed component relative angles between the previous rigid body and rigid body. The method also includes receiving an angular velocity of the previous rigid body', an angular velocity of the rigid body, a rotation matrix from the previous rigid body' to the rigid body, and at least one covariance matrix. First and second component relative angles between the previous rigid body and the rigid body are calculated based on the first and second observed component relative angles, the angular velocity of the previous rigid body, the angular velocity of the rigid body, the rotation matrix from the previous rigid body to the rigid body, and the at least one covariance matrix.

[0016] In an embodiment, receiving the at least one covariance matrix includes receiving at least one of an initial estimation error covariance matrix, a process noise covariance matrix, or an observ ation noise covariance matrix.

[0017] In another embodiment, the method also includes determining a relative angle between the rigid body and a next rigid body without using the first and second component relative angles between the previous rigid body and the rigid body.

[0018] In accordance with yet another embodiment, a method for determining relative angles between pivotally coupled rigid bodies arranged in series includes receiving a gravitational acceleration vector and an angular velocity of a previous rigid body determined from observations of an IMU mounted on the previous rigid body, and receiving a gravitational acceleration vector and an angular velocity of a rigid body determined from observations of an IMLT mounted on the rigid body. A relative angle between the previous rigid body and the rigid body is estimated based on the gravitational acceleration vector and the angular velocity of the previous rigid body and the gravitational acceleration vector and the angular velocity of the rigid body. The relative angle between the previous rigid body and the rigid body is determined without using an absolute angle of the previous rigid body or an absolute angle of the rigid body.

BRIEF DESCRIPTION OF THE DRAWINGS

[0019] FIG. 1 is a simplified side view of an excavator (an example of an object having a series of coupled rigid bodies) in accordance with an embodiment;

[0020] FIG. 2 is a flowchart illustrating a method of determining an observed component relative angle between rigid bodies in accordance with an embodiment;

[0021] FIG. 3 is a flowchart illustrating a method of determining a component relative angle between rigid bodies in accordance with an embodiment; [0022] FIG. 4 is a flowchart illustrating a method of determining a component relative angle between rigid bodies in accordance with some embodiments;

[0023] FIG. 5 is a flowchart illustrating methods of determining component relative angles between a series of rigid bodies in accordance with some embodiments;

[0024] FIG. 6 is a simplified diagram of a control system in accordance with some embodiments;

[0025] FIG. 7 is a flowchart illustrating a method of determining component relative angles between a previous rigid body and a rigid body in accordance with an embodiment;

[0026] FIG. 8 is a flowchart illustrating a method of determining a relative angle between a rigid body and a next rigid body in accordance with an embodiment;

[0027] FIG. 9 is a flowchart illustrating a method of determining component relati ve angles between rigid bodies arranged in a series in accordance with an embodiment; and

[0028] FIG. 10 is a flowchart illustrating a method of determining relative angles between pivotally coupled rigid bodies arranged in a series in accordance to an embodiment.

DETAILED DESCRIPTION

[0029] Embodiments described herein provide improved methods of determining relative angles and/or component relative angles between coupled rigid bodies. In some embodiments, for example, a component relative angle can be determined based on measurements and calculated parameters from the rigid body and an immediately prior rigid body in a series without requiring information from earlier rigid bodies in the series.

[0030] An example of a series of rigid bodies that will be used throughout this application is an excavator that includes a number of movable parts (or bodies) that are pivotally coupled together. It should be appreciated that an excavator is used merely as an example, and the embodiments described herein may be used with any other heavy equipment, vehicles, machinery, or objects that include rigid bodies coupled in series. [0031] FIG. 1 is a simplified side view of an excavator that includes a platform 11, a boom 12, a stick 16, and a bucket 20. The boom 12 is pivotally coupled to the platform 11 at a pivot point 14, the stick 16 is pivotally coupled to the boom 12 at a pivot point 18, and the bucket 20 is pivotally coupled to the stick 16 at a pivot point 22. Hydraulic devices 24, 26, 28 are provided to move the boom 12, the stick 16, and the bucket 20. The bucket 20 includes teeth 30 that may assist in digging. The platform 11 includes a cab 31 supported on an undercarriage 32 that may include wheels or tracks to facilitate movement of the excavator over a worksite. The platform 1 1 can be rotated about a generally vertical axis 35 by a hydraulic motor 33. It should be appreciated that although this example includes a one-piece boom, embodiments described herein can be utilized with excavators having variable angle booms. Further, the excavator can be used with other implements or tools besides the bucket 20 such as augers, trenchers, compactcors, and the like.

[0032] Excavators commonly utilize a variety of sensors to monitor positions of various machine elements and/or to provide a display of the element positions to an operator. As an example, relative angles between the platform 11, the boom 12, the stick 16, and the bucket 20 can be determined using encoders; sensors associated with pivot points 14, 18, 22; string encoders associated with hydraulic devices 24, 26, 28; and/or other sensors. Alternatively, angles of the bodies can be determined relative to gravity using inclinometers such as IMUs. In the example of FIG. 1, the excavator includes an IMU 42 on the platform 11, an IMU 44 on the boom 12, an IMU 46 on the stick 16, and an IMU 48 on the bucket 20. These IMUs can be used to determine, for example, gravitational acceleration and angular velocity of the rigid bodies. The IMUs may operate in one or multiple axes.

[0033] The excavator in this example includes a controller 50 having an associated memory. The controller 50 may be responsive to the IMUs for determining a position of the bucket 20 and/or teeth 30 based on the relative angles of the bodies. The position can be determined relative to the platform 11 or a point on the platform 11, or the platform 11 may include a position sensor, such as Global Navitation Satellite Systen (GNSS) positioning system 34, that allows the position to be determined in another coordinate frame.

[0034] The controller 50 may also determine a difference between the position of the bucket 20 and a desired elevation at a digging point on the worksite. The desired elevation may be determined from a desired topology that is stored in memory. An operator may control movement of the excavator using controls in the cab 31 and/or the controller 50 may provide automated machine control. A display 78 in the cab 31 may assist the operator by showing a current position of the bucket 20 and a desired elevation. The automated machine control may move the parts or bodies based on the position of the bucket 20 and/or teeth 30 relative to the desired elevation.

[0035] FIG. 2 is a flowchart illustrating a method of determining observed component relative angles sin (a _n ) and cos(d _n ) between a rigid body n and a previous rigid body n-1 in accordance with an embodiment. The rigid body n and the previous rigid body n-1 in this example are rigid bodies that are pivotally coupled. In one embodiment, the observed component relative angles sin (<x„) and cos(d„) are calculated, at a relative angle observation modue, based on a gravitational acceleration vector AccG _n-x of the previous rigid body n-1 and a gravitational acceleration vector AccG _n of the rigid body n. In one example, the gravitational acceleration vector AccG is a 3D vector with elements x, y, and z. The elements are typically aligned to x, y, and z axes of the corresponding rigid body.

[0036] Generally, the gravitation acceleration is not measured directly by an IMU. Instead, the gravitation acceleration, or a gravitational acceleration vector AccG _n , can be determined using known techniques from IMU observations.

[0037] The observed component relative angles sin (a _n ) and cos(a _n ) between the rigid body n and the previous rigid body n-1 may be used in a relative angle estimation module as described in FIG. 3 to determine estimated component relative angles sin(d _n ) and cos(d _n ).

The estimated component relative angles may be more accurate than the observed component relative angles.

[0038] In a particular embodiment, the observed component relative angles

sin(a _n ) and cos(d _n ) may be calculated using observations from an IMU _;; mounted on the rigid body n and an IMU«-; mounted on the previous rigid body n-1 and the following equations:

AccG _n -- _i (x)AccG _n (x}+AccG _n® (y)AccG _n (y) Equation (1) cos(fi _n ) =

AccG _n (x) ² +AccG _n (y) ²

[0039] At the relative angle estimation module of FIG. 3, an angular velocity (g _n ) of the rigid body », an angular velocity' ) of the previous rigid body n-1, a rotation matrix (R _n -i) from the previous rigid body n-1 to the current rigid body n, a process noise covariance matrix in Kalman Filter parameter, an observation noise covariance matrix in Kalman Filter parameter, and an initial estimation error covariance matrix in Kalman Filter parameter (Q, R, P), respectively, are used along with observed component relative angles sin (a _n ) and cos(d _n ) to determine component relative angle estimates sin(d _n ) and cos(d _n ). [0040] In a particular· embodiment, a first relative angle rate w is calculated using the following equations:

Equation (2) with a process model:

Equation (3) and the observation model:

Equation (4) and Q, R, and P as process noise covariance matrix, observation noise covariance matrix, and initial estimation error covariance matrix based on Kalman Filter parameters, respectively.

[0041] Defining the following state vector for the Kalman Fiter: sin(d _n )

X = Equation (5 ) cos(d„) _. and the observation vector: sin(a„)

Z A Equation (6) cos (a _n )

The time update of the Kalman Filter is:

Equation (7) and the measurement update of the Kalman Filter is:

Equation (8) where superscripts“+” and represent posteriori and priori estimates respectively. K (t) is the Kalman gain, and superscript‘T’ is the matrix transpose operator. At each time epoch, X ⁺ is the estimation vector for sin(d„) and cos(d„). [0042] FIG. 4 is a flowchart illustrating a method of determining component relative angles sin(d„) and cos(d _n ) between a rigid body n and a previous rigid body n-1 in accordance with some embodiments. In one example, the component relative angles are a sine component angle and a cosine component angle. This flowchart shows how the inputs AccG _n® ; AccG _n ; g- _n -^, g _n R _n ’-X·, and Q, R, P can be used to determine the component relative angles

sin(d _n ) and cos(a _n ) at the rigid body . This flowchart also shows how outputs AccG _n and g _n can be passed to a next rigid body n+1. As with other embodiments, this figure shows that the component relative angles at a rigid body n can be determined based on IMU observations from the rigid body n and the previous rigid body «-7 without requiring any absolute angles.

[0043] FIG. 5 is a flowchart illustrating methods of determining component relative angles between rigid bodies in accordance with an embodiment. This figure shows three boxes representing the calculation of component relative angles at Rigid Bodyi, Rigid Body , and Rigid Body,- . Although not specifically shown, each of the rigid bodies have an IMU mounted thereon and are arranged in series similar to the examples shown in FIG. 1. [0044] Each of the boxes in FIG. 5 has a number of inputs that are used to calculate the component relative angles. The inputs are represented by arrows pointing inward toward the box. The inputs shown on the left of each box are those that are associated with a previous rigid body in the series. The inputs shown on the top of each box are those that are associated with a current rigid body.

[0045] Outputs are represented by arrows pointing away from the box. The outputs shown on the right of each box are values that are passed from the current rigid body to a next rigid body in the series. The outputs shown on the bottom of each box are the component relative angles. [0046] For each rigid body (except the first rigid body in the series or the 1MU coupled to the first rigid body), the inputs, determined from observations of an IMU associated with a previous rigid body, are: g _n - is an angular velocity vector of the previous rigid body ;

AccG _n- is a gravitational acceleration vector of the previous rigid body.

[0047] For each rigid body, the inputs, determined from observations of an IMU associated with the rigid body, are: g _n is an angular velocity vector of the current rigid body; and

AccG _n is a gravitational acceleration vector of the current rigid body;

[0048] The outputs that are passed from the current rigid body' to the next rigid body are: g _n is the angular velocity vector of the current rigid body; and

AccG _n is the gravitational acceleration vector of the current rigid body.

[0049] The prev ious rigid body is the immediately prior rigid body in the series, and the next rigid body is the immediately following rigid body' in the series. In some embodiments, the relative angle between the rigid body (i.e., rigid body n) and the previous rigid body (i.e., rigid body' n-1) is represented by the estimated relative angle a _n and further represented by estimated component relative angles sin(d _n ) and cos(d _n ). The gravitational acceleration vector and angular v elocity at the rigid body n can be used as some of the inputs to determine the estimated component relative angles sin(a _n ) and cos(<2 _n ). As shown in this example, the component relative angles sin(a _n ) and cos (a _n ) can be determined based on information from the previous rigid body and the rigid body without requiring information from earlier rigid bodies in the series. This example also shows that absolute angles are not required to determine the estimated component relative angles sin(d _n ) and cos ( _n ).

[0050] In some embodiments, the component relative angles sin(cr _n ) and cos(a _n ) between a rigid body n and a previous rigid body n-1 are calculated based on (i) observed component relative angles sin (a _n ) and cos ( _n ): (ii) an angular velocity (g _n ) of the rigid body n; (iii) an angular velocity ( „_ , ) of the previous rigid body n-l a rotation matrix ( /? "_ ) from the previous rigid body to the current rigid body; a process noice covariance matrix in Kalman Filter parameter, an observation noise covariance matrix in Kalman Filter parameter, and an initial estimation error covariance matrix in Kalman Filter parameter (Q, R, P), respectively. This is shown, for example, in FIG. 3. Once again, the component relative angles are determined based on measurements and calculated paramerters from the rigid body n and the previous rigid body n-1 without requiring information from earlier rigid bodies in the series or absolute angles of the rigid bodies.

[0051] FIG. 6 is a simplified diagram of a control system in accordance with some embodiments. The system includes a state estimator that may be used, for example, with the excavator shown in FIG. 1 (e.g., controller 50). This figure show's how outputs from IMUs associated with a platform, a boom, a stick, and a bucket are passed to the state estimator. The IMUs provide, for example, measured accelerations and angular velocities to the state estimator. The state estimator uses the measured values, along with other inputs as described in the various embodiments, to determine relative angles and/or component relative angles. In some embodiments, the state estimator may also determine a position of an implement (e.g., a bucket and/or teeth) as described herein. These values and/or positions may be output on a display to assist an operator.

[0052] The state estimator and/or controller typically includes familiar software and hardware components. For example, it may include one or more operating systems, processors, local memories for storage, I/O devices, and system buses interconnecting the hardware components. RAM and disk drives are examples of local memory for storage of data and computer programs. Other types of local memories include magnetic storage media, optical storage media, flash or solid state memories, networked storage devices, and the like.

[0053] FIG. 7 is a flowchart illustrating a method of estimating relative angles between rigid bodies in accordance with some embodiments. The rigid bodies may be coupled in a series similar to the rigid bodies of the excavator shown in FIG. 1. The method includes obtaining a gravitational acceleration vector and angular velocity of a previous rigid body from

observations of an IMU mounted on the previous rigid body 701. The previous rigid body is the immediately prior rigid body in the series of rigid bodies. The method also includes obtaining a gravitational acceleration vector and angular velocity' of a current rigid body from observations of an IMU mounted on the current rigid body 702.

[0054] The method also includes estimating a first component relative angle and a second component relative angle between the previous rigid body and the current rigid body based at least in part on the gravitational acceleration vector and the angular velocity of the previous rigid body and the gravitational acceleration vector and the angular velocity of the current rigid body 703. As described previously, the component relative angles can be determined based on the gravitational acceleration vector of the previous rigid body, the gravitational acceleration vector of the current rigid body, the angular velocity of the previous rigid body, and the angular velocity' of the current rigid body. As described previously, estimating the component relative angles can also include a rotation matrix from the previous rigid body to the current rigid body, intermediate observ ed first and second component relative angles between the previous rigid body and the current rigid body, a process noise covariance with Kalman Filter parameter, an observation noise covariance with Kalman Filter parameter, and an initial estimation error covariance with Kalman Filter parameter.

[0055] FIG. 8 is a flowchart illustrating a method of estimating relative angles between rigid bodies in accordance with some embodiments. The rigid bodies may be coupled in a series similar to the rigid bodies of the excavator shown in FIG. I . The method includes receiving a gravitational acceleration vector and an angular velocity of a rigid body from observations of an IMU mounted on the rigid body 801. The method also includes receiving a gravitational acceleration vector and an angular velocity' of a next rigid body from observations of an IMU mounted on the rigid body 802. The next rigid body is the immediately next rigid body in the series of rigid bodies.

[0056] The method also includes estimating first and second component relative angles between the rigid body and the next rigid body based at least in part on the gravitational acceleration vector and the angular velocity of the rigid body and the gravitational acceleration vector and the angular velocity of the next rigid body 803. As described previously, the component relative angles can be determined based on the gravitational acceleration vector of the rigid body, the gravitational acceleration vector of the next rigid body, the angular velocity of the rigid body, and the angular velocity of the next rigid body. As described previously, estimating the component relative angles can also include a rotation matrix from the rigid body to the next rigid body, intermediate observed first and second component relative angles between the rigid body and the next rigid body, a process noise covariance with Kalman Filter parameter, an observation noise covariance with Kalman Filter parameter, and an initial estimation error covariance with Kalman Filter parameter.

[0057] FIG. 9 is a flowchart illustrating a method of estimating relative angles between rigid bodies based on observed component relative angles sin (d _n ) and cos(d _n ) between rigid bodies (i.e. a previous rigid body and a rigid body) in accordance with some embodiments.

The rigid bodies may be coupled in a series similar to the rigid bodies of the excavator shown in FIG. 1. The method includes receiving a gravitational acceleration vector of a previous rigid body and a gravitational acceleration vector of a rigid body 901. The method also includes determining a first observed component relative angle of the rigid body and a second observed component relative angle of the rigid body 902. The method also includes estimating a first component relative angle and a second component relative angle between the previous rigid body and the rigid body based at least in part on the gravitational acceleration vector and the angular velocity- of the previous rigid body, the gravitational acceleration vector and the angular velocity' of the rigid body, and the first observed component relative angle of the rigid body and the second observed component relative angle of the rigid body 903. As described previously, the observed component relative angles can be determined based on the gravitational acceleration vector of the previous rigid body and the gravitational acceleration vector of the rigid body. Estimating the component relative angles based on the observed component relative angles and may include a rotation matrix from the previous rigid body to the rigid body, a process noise covariance with Kalman Filter parameter, an observation noise covariance with Kalman Filter parameter, and an initial estimation error covariance with Kahnan Filter parameter.

[0058] FIG. 10 is a flowchart illustrating a method of determining angles between pivotally coupled rigid bodies arranged in a series in accordance with some embodiments. The series includes a rigid body arranged between a previous rigid body and a next rigid body similar to some of the rigid bodies of the excavator shown in FIG. 1. The method includes, based on an arrangement of rigid bodies coupled in a series that includes a rigid body coupled to a previous rigid body, estimating a relative angle between the previous rigid body and rigid body without using an absolute angle of the previous rigid body or an absolute angle of the rigid body as inputs 1001.

[0059] It should be appreciated that the specific steps illustrated in FIGS. 7-10 provide particular methods according to some embodiments. Other sequences of steps may also be performed according to alternative embodiments. For example, alternative embodiments may perform the steps outlined above in a different order. Moreover, the individual steps illustrated in FIGS. 7-10 may include multiple sub-steps that may be performed in various sequences. Furthermore, additional steps may be added or removed depending on the particular application.

[0060] It should be appreciated that some embodiments may be implemented by hardware, software, firmware, middleware, microcode, hardware description languages, or any combination thereof. When implemented in software, firmware, middleware, or microcode, the program code or code segments to perform the necessary tasks may be stored in a computer- readable medium such as a storage medium. Processors may be adapted to perform the necessary tasks. The term“computer-readable medium” includes, but is not limited to, portable or fixed storage devices, optical storage devices, wireless channels, sim cards, other smart cards, and various other non-transitory mediums capable of storing, containing, or carrying instructions or data. [0061] While the present invention has been described in terms of specific embodiments, it should be apparent to those skilled in the art that the scope of the present invention is not limited to the embodiments described herein. For example, features of one or more

embodiments of the invention may be combined with one or more features of other

embodiments without departing from the scope of the invention. The specification and drawings are, accordingly, to be regarded in an illustrative rather than a restrictive sense. Thus, the scope of the present invention should be determined not with reference to the above description, but with reference to the appended claims along with their full scope of equivalents.