End Effector Position Estimation

Abstract

One example is directed to an end effector position estimation system for an off-road vehicle, which includes at least one inertial measurement unit (IMU) configured to be positioned on at least one of actuators and links of the vehicle that together move the end effector of the vehicle, and configured to generate measurement signals. The position estimation system includes at least one other IMU configured to be positioned on a base of the vehicle, and configured to generate other measurement signals. The position estimation system includes an estimation unit to estimate a position of the end effector of the vehicle based at least in part on the measurement signals and the other measurement signals, wherein the estimation unit is configured to perform an estimation method that removes an influence of terrain-induced vibrations and terrain slope in the measurement signals based on the other measurement signals.

Claims

1. An end effector position estimation system for an off-road vehicle, comprising: at least one inertial measurement unit (IMU) configured to be positioned on at least one of actuators and links of the vehicle that together move the end effector of the vehicle, and configured to generate measurement signals; at least one other IMU configured to be positioned on a base of the vehicle, and configured to generate other measurement signals; and an estimation unit to estimate a position of the end effector of the vehicle based at least in part on the measurement signals and the other measurement signals, wherein the estimation unit is configured to perform an estimation method that removes an influence of terrain-induced vibrations and terrain slope in the measurement signals based on the other measurement signals.

2. The end effector position estimation system of claim 1, wherein the estimation method includes adaptive feedforward disturbance removal.

3. The end effector position estimation system of claim 1, wherein the estimation unit estimates a real-time rotation or translation of each of the actuators and links in order to estimate the position of the end effector.

4. The end effector position estimation system of claim 1, wherein the off-road vehicle is an agricultural vehicle.

5. The end effector position estimation system of claim 1, wherein the off-road vehicle is a construction vehicle.

6. The end effector position estimation system of claim 1, and further comprising: at least one range or linear translational position sensor to generate range information based on translation motions of at least one link of the vehicle with linear translational degrees of freedom; and wherein the estimation unit is configured to estimate the position of the end effector based further on the generated range information.

7. The end effector position estimation system of claim 6, wherein the at least one range or linear translational position sensor includes a low-cost laser sensor.

8. The end effector position estimation system of claim 6, and further comprising: a combination of an inexpensive low bandwidth range sensor located on a translating arm of the vehicle together with an IMU located at a moving end of the translating arm to generate together an estimate of a translational motion of at least one link of the vehicle with linear translational degrees of freedom; and wherein the estimation unit is configured to estimate the position of the end effector based further on the generated estimate of translation motion.

9. The end effector position estimation system of claim 1, wherein the estimation unit is configured to determine whether the vehicle is operating within safe operating limits based at least in part on the estimated position of the end effector.

10. The end effector position estimation system of claim 9, wherein the estimation unit is configured to determine whether the vehicle is operating within safe operating limits based further on an estimated weight of a load carried by the end effector.

11. The end effector position estimation system of claim 1, wherein the system is configured to control the vehicle to prevent vehicle tip over based at least in part on the estimated position of the end effector.

12. The end effector position estimation system of claim 1, wherein the system is configured to prevent motion of the end effector into regions that may cause the vehicle to tip-over.

13. The end effector position estimation system of claim 1, wherein the system is configured to prevent motion of the end effector into a restricted region.

14. The end effector position estimation system of claim 13, wherein the restricted region comprises a wall, and wherein the system is configured to prevent the end effector from contacting the wall.

15. An end effector position estimation system for an off-road vehicle, comprising: at least one inertial measurement unit (IMU) to be positioned on at least one of actuators and links of the vehicle that together move the end effector of the vehicle, and to generate measurement signals; at least one other IMU to be positioned on a base of the vehicle, and to generate other measurement signals; at least one laser range sensor to generate range information based on translation motion of at least one of the links of the vehicle with linear translational degrees of freedom; and an estimation unit to estimate real-time rotation or translation of each of the actuators and links based on the measurement signals, other measurement signals, and range information, and to estimate a position of the end effector of the vehicle based on the estimated real-time rotation or translation of each of the actuators and links.

16. The end effector position estimation system of claim 15, wherein the estimation unit removes an influence of terrain-induced vibrations and terrain slope in the measurement signals based on the other measurement signals.

17. The end effector position estimation system of claim 15, wherein the estimation unit determines whether the vehicle is operating within safe operating limits based at least in part on the estimated position of the end effector.

18. The end effector position estimation system of claim 15, wherein the system controls the vehicle to prevent vehicle tip over based on the estimated position of the end effector and an estimated weight of a load carried by the end effector.

19. A method, comprising: generating measurement signals with at least one inertial measurement unit (IMU) positioned on at least one of actuators and links of an off-road vehicle that together move an end effector of the vehicle; generating other measurement signals with at least one other IMU configured to be positioned on a base of the vehicle; compensating the measurement signals based on the other measurement signals; and estimating a position of the end effector of the vehicle based at least in part on the compensated measurement signals.

20. The method of claim 19, and further comprising: determining whether the vehicle is operating within safe operating limits based at least in part on the estimated position of the end effector.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0011] The accompanying drawings are included to provide a further understanding of embodiments and are incorporated in and constitute a part of this specification. The drawings illustrate embodiments and together with the description serve to explain principles of embodiments. Other embodiments and many of the intended advantages of embodiments will be readily appreciated as they become better understood by reference to the following detailed description. The elements of the drawings are not necessarily to scale relative to each other. Like reference numerals designate corresponding similar parts.

[0012] FIG. 1 is a block diagram illustrating a system for estimating position according to one example.

[0013] FIG. 2 is a diagram illustrating a compact wheel loader vehicle with a position estimation system that measures two rotational motions to determine end effector position according to one example.

[0014] FIG. 3A is a diagram illustrating a commercial track loader vehicle with a position estimation system that measures two rotational motions to determine end effector position according to one example.

[0015] FIG. 3B is a diagram illustrating a zoomed-in view of a portion of the lift actuator of the commercial track loader vehicle shown in FIG. 3A according to one example.

[0016] FIG. 3C is a diagram illustrating a zoomed-in view of a portion of the tilt actuator of the commercial track loader vehicle shown in FIG. 3A according to one example.

[0017] FIG. 4A is a diagram illustrating a compact track loader vehicle with a position estimation system that measures one rotational motion and one translational motion to determine end effector position according to one example.

[0018] FIG. 4B is a diagram illustrating a lift hydraulic actuator and a tilt hydraulic actuator of the compact track loader vehicle shown in FIG. 4A according to one example.

[0019] FIG. 5 is a diagram illustrating definitions of tilt angle, θ, and roll angle, ϕ, of terrain according to one example.

[0020] FIG. 6 is a diagram illustrating definitions of inertial tilt angles of a vehicle base, of a tilt actuator, and of a lift actuator according to one example.

[0021] FIG. 7 is a diagram illustrating a vehicle with positions of points on links and of an end effector according to one example.

[0022] FIG. 8 is a diagram illustrating a vehicle with a tip-over prevention system according to one example.

[0023] FIG. 9 is a flow diagram illustrating a method for estimating position of an end effector according to one example.

[0024] FIG. 10 is a flow diagram illustrating a method for tip-over prevention of a vehicle according to one example.

[0025] FIG. 11 is a diagram illustrating an excavator vehicle with three lengths and three tilt angles that may be used to estimate a position of the end effector according to one example.

[0026] FIG. 12 is a diagram illustrating the excavator vehicle shown in FIG. 11 with a position estimation system that measures three rotational motions to determine the end effector position according to one example.

[0027] FIG. 13 is a diagram illustrating vibration and slope compensation according to one example.

DETAILED DESCRIPTION

[0028] In the following Detailed Description, reference is made to the accompanying drawings, which form a part hereof, and in which is shown by way of illustration specific embodiments in which the invention may be practiced. In this regard, directional terminology, such as “top,” “bottom,” “front,” “back,” “leading,” “trailing,” etc., is used with reference to the orientation of the Figure(s) being described. Because components of embodiments can be positioned in a number of different orientations, the directional terminology is used for purposes of illustration and is in no way limiting. It is to be understood that other embodiments may be utilized and structural or logical changes may be made without departing from the scope of the present invention. The following detailed description, therefore, is not to be taken in a limiting sense, and the scope of the present invention is defined by the appended claims.

[0029] It is to be understood that the features of the various exemplary embodiments described herein may be combined with each other, unless specifically noted otherwise.

[0030] Some examples disclosed herein are directed to an inertial measurement unit based end effector position estimation and tip-over prevention system. FIG. 1 is a block diagram illustrating a system 100 for estimating position according to one example. System 100 includes a plurality of inertial measurement units (IMUs) 102, a laser range finder sensor 104, and a computing device 106. Individual ones of the IMUs 102 may be individually addressed herein with an appended number after “102” (e.g., 102(1), 102(2), etc.). The computing device 106 includes a processor 108 and a memory 110. Memory 110 stores sensor data processing module 112.

[0031] Depending on the exact configuration and type of computing device, the memory 110 may be volatile (such as RAM), non-volatile (such as ROM, flash memory, etc.), or some combination of the two. The memory 110 used by computing device 106 is an example of computer storage media (e.g., non-transitory computer-readable storage media storing computer-executable instructions for performing a method). Computer storage media used by computing device 106 according to one example includes volatile and nonvolatile, removable and non-removable media implemented in any suitable method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. Computer storage media includes, but is not limited to, RAM, ROM, EEPROM, flash memory or other memory technology, CD-ROM, digital versatile disks (DVD) or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to store the desired information and that can be accessed by processor 108.

[0032] In some examples, system 100 utilizes IMUs 102 to provide sensor information for estimation of rotational and translational variables. Each IMU 102 may be implemented as an IMU sensor chip that includes a 3-axis accelerometer and a 3-axis gyroscope. In some examples, the IMUs 102 may also each include a 3-axis magnetometer. Thus, each IMU 102 provides either six or nine measurement signals (i.e., three accelerations from the three accelerometers, three rotational rates from the three gyroscopes, and, in some examples, three magnetic field intensities from the three magnetometers). In some examples, the IMU sensor chip may include a microcontroller, a battery, and a wireless transceiver (e.g., Bluetooth) and antenna. In some examples, the IMU sensor chip may include an Inter-Integrated Circuit (I2C) or Serial Peripheral Interface (SPI) communications interface. The accelerometer in each IMU 102 may measure both linear acceleration and gravity. Measurements of gravity in static positions allow estimation of vertical tilt angle. Measurements of the rotation rates allow dynamic estimation of tilt angle.

[0033] IMUs 102 and laser sensor 104 output sensor information to computing device 106. Processor 108 executes sensor data processing module 112 to perform a position estimation method, and other methods disclosed herein, using the received sensor information. Sensor data processing module 112 outputs position information 120 and alerts and control information 122, based on the processing of the received sensor information. In some examples, system 100 may be used as an IMU-based end effector position estimation system for agricultural vehicles, construction vehicles, and other off-road vehicle systems, and the position information 120 indicates a current position of the end effector. In some examples, the system 100 estimates the position of an end effector of the vehicle, and also detects if a vehicle tip-over is likely to occur, so that such a tip-over event can be prevented. The alerts and control information 122 may include an audible and/or visual alert that a tip-over event is about to occur so that an operator can prevent the occurrence, and/or it may include control information to automatically control the vehicle to prevent the tip-over event from occurring.

[0034] At least one of the IMUs 102 may be located on actuators and/or links that together move the end effector of the vehicle. At least one of the IMUs 102 may be located on a base of the vehicle. The position estimation method performed by sensor data processing module 112 may include removing the influence of terrain induced vibrations and terrain slope (e.g., measured by an IMU 102 on the vehicle base) from the signals of the IMUs 102 located on the actuators or links. In some examples, the position estimation method includes adaptive feedforward disturbance removal and other signal processing methods. Sensor data processing module 112 may estimate the real-time rotation or translation of each actuator or link of the vehicle, in order to estimate the end effector position. Sensor data processing module 112 may also determine if the end effector position and the weight of the load carried by the end effector will cause vehicle tip-over.

[0035] FIG. 2 is a diagram illustrating a compact wheel loader vehicle 200 with a position estimation system that measures two rotational motions to determine end effector position according to one example. Compact wheel loader vehicle 200 includes two hydraulic actuators 202 and 204 to move the end effector 206, which is a bucket in this example. The two hydraulic actuators include a lift hydraulic actuator 202 and a tilt hydraulic actuator 204. Each of the two actuators 202 and 204 has a rotational motion as part of a kinematic mechanism while it is being actuated. A first IMU 102(1) is positioned on the lift hydraulic actuator 202, and a second IMU 102(2) is positioned on the tilt hydraulic actuator 204. The IMUs 102(1) and 102(2) on the actuators 202 and 204, respectively, may be used to estimate their real-time inertial rotational angles. In addition, a reference IMU 102(3) located on the vehicle base may be used to measure reference signals that can be used to compensate for terrain slope and terrain induced vibrations. Other elements of system 100 (FIG. 1), such as computing device 106, may also be positioned on or in vehicle 200.

[0036] FIG. 3A is a diagram illustrating a commercial track loader 300 with a position estimation system that measures two rotational motions to determine end effector position according to one example. FIG. 3B is a diagram illustrating a zoomed-in view of a portion 308 of the lift actuator of the commercial track loader vehicle 300 shown in FIG. 3A according to one example. FIG. 3C is a diagram illustrating a zoomed-in view of a portion 310 of the tilt actuator of the commercial track loader vehicle 300 shown in FIG. 3A according to one example.

[0037] Commercial track loader vehicle 300 includes two hydraulic actuators 302 and 304 to move the end effector 306, which is a bucket in this example. The two hydraulic actuators include a lift hydraulic actuator 302 and a tilt hydraulic actuator 304. The two actuators 302 and 304 undergo rotational motion when they are used to move the end effector 306. Hence, measuring the rotational angles on these two actuators provides information to estimate the end effector position. As shown in FIG. 3B, a first IMU 102(4) is positioned on the lift hydraulic actuator 302 to measure rotational angle 312 of the lift hydraulic actuator 302. As shown in FIG. 3C, a second IMU 102(5) is positioned on the tilt hydraulic actuator 304 to measure rotational angle 314 of the tilt hydraulic actuator 304. The IMUs 102(4) and 102(5) on the actuators 302 and 304, respectively, may be used to estimate their real-time inertial rotational angles, and based on this information, the position of the end effector 306 may be determined by system 100.

[0038] While the IMUs 102 on actuators or links can estimate rotational angles of these components, such IMUs 102 may also measure the influence of terrain induced vehicle vibrations. Such terrain-induced vehicle vibrations may cause errors in the estimated orientation angles. Some examples disclosed herein detect and compensate for such terrain induced vehicle vibrations.

[0039] FIG. 4A is a diagram illustrating a compact track loader vehicle 400 with a position estimation system that measures one rotational motion and one translational motion to determine end effector position according to one example. Compact track loader vehicle 400 includes a pair of lift hydraulic actuators 402 and a pair of tilt hydraulic actuators 404 to move the end effector 406, which is a bucket in this example. The tilt hydraulic actuators 404 undergo rotational motion, which also causes equivalent rotational motion of the lift hydraulic actuators 402. In addition, the lift hydraulic actuators 402 cause translational motion of the end effector 406. Thus, in this case, the rotational tilt motion and the translational motion of the piston rod of the lift hydraulic actuators 402 are estimated by system 100.

[0040] FIG. 4B is a diagram illustrating a lift hydraulic actuator 402 and a tilt hydraulic actuator 404 of the compact track loader vehicle 400 shown in FIG. 4A according to one example. As shown in FIG. 4B, a first IMU 102(6) located inside a telescoping arm 408 may be used to estimate tilt angle, θ, while a laser sensor 104(1) at the same location, which measures the current length, l, of the telescoping arm 408, together with a second IMU 102(7) located at the end of the piston rod 410 of the lift hydraulic actuator 402, which measures linear acceleration, a.sub.x, may all be used to estimate bucket position.

[0041] In some examples, sensor data processing module 112 (FIG. 1) performs a method of estimating tilt angle of an actuator using an IMU 102. The module 112 may use the known direction and magnitude of gravity and the components of static acceleration values along the 3-axes of the IMU sensor 102 to calculate orientation of the IMU 102 with respect to gravity. This provides both vertical tilt angles of the link or actuator on which the IMU 102 is located. The module 112 may combine in a Kalman Filter (or other estimator) the static vertical angles from the accelerometers with the gyroscope signals along both these axes to calculate vertical tilt angles that are both statically correct and have high bandwidth. Additional details related to this method may be found in Y. Wang and R. Rajamani, “Direction Cosine Matrix Estimation with an Inertial Measurement Unit,” Mechanical Systems and Signal Processing, Vol. 109, pp. 268-284, 1 Sep. 2018, which is hereby incorporated by reference herein.

[0042] To determine the vertical tilt angles from the accelerometer signals, the sensor data processing module 112 finds the third column of the direction cosine matrix (DCM) using the following Equation I:

[00001]$\begin{array}{cc}{v}_3=-\frac{1}{g}\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]& \mathrm{Equation}I\end{array}$ [0043] where: a.sub.x, a.sub.y and a.sub.z are measured accelerations along the sensor axes.

[0044] The third column of the DCM is given by the following Equation II:

[00002]$\begin{array}{cc}{v}_3=\left[\begin{array}{c}-\sin \left({\theta }_{\mathrm{accels}}\right)\\ \cos \left({\theta }_{\mathrm{accels}}\right)\sin \left({\varphi }_{\mathrm{accels}}\right)\\ \cos \left({\varphi }_{\mathrm{accels}}\right)\cos \left({\theta }_{\mathrm{accels}}\right)\end{array}\right]& \mathrm{Equation}\mathrm{II}\end{array}$

[0045] The two vertical angles θ.sub.accels and ϕ.sub.accels can be obtained from the estimate of v.sub.3. Here, θ.sub.accels is the vertical angle of interest (i.e., the tilt angle). The static tilt angle θ.sub.accels from the gravity component can be combined with the gyroscope measurement to obtain a drift-free high bandwidth tilt angle as shown in the following Equation III:

{circumflex over ({dot over (θ)})}={grave over (θ)}.sub.gyro+(θ.sub.accels−{circumflex over (θ)})   Equation III

[0046] FIG. 5 is a diagram illustrating definitions of tilt angle, θ, and roll angle, ϕ, of terrain according to one example. FIG. 6 is a diagram illustrating definitions of inertial tilt angles of a vehicle base, of a tilt actuator, and of a lift actuator according to one example. As shown in FIG. 6, IMU 102(3) is positioned on the vehicle base. The IMU 102(3) may be utilized to estimate the tilt angle of the base, θ.sub.base. The IMU 102(2) on the tilt actuator (or link) 204 may be used to estimate the inertial tilt angle of the tilt arm, θ.sub.tilt. The IMU 102(1) on the lift actuator (or link) 202 may be used to estimate the inertial tilt angle of the lift link, θ.sub.lift.

[0047] In some examples, sensor data processing module 112 (FIG. 1) performs a method of removing the influence of terrain-induced slope and vibrations on acceleration signals. The module 112 may use the accelerometers and gyroscopes of the IMU 102(3) on the vehicle base to provide a reference signal related to upstream measurement of terrain-induced disturbances that influence the accelerometers and gyroscopes on the actuator. The module 112 may use an adaptive feedforward least-mean-squares algorithm to remove the influence of the disturbances in real-time on the actuator-mounted sensors. For this method, θ.sub.base-lowpass is the estimated lowpass filtered tilt angle of the base. This variable retains the information of road slope and static angle of the base, but does not retain high frequency vibrations. For this method, {dot over (θ)}.sub.base is the gyroscope measured angular rate of the base. This signal retains both the influence of road slope and of road-induced vibrations. The following base tilt angle estimate is then utilized as the reference:

{circumflex over ({dot over (θ)})}.sub.base={grave over (θ)}.sub.base+l.sub.1(θ.sub.base-lowpass−{circumflex over (θ)})   Equation IV

[0048] Let θ.sub.actuator be the estimated inertial tilt angle of the actuator. Then the road induced disturbance on the actuator can be estimated as follows.

[0049] A reference road-disturbance influence vector is obtained as shown in the following Equation V:

x(n)=[{circumflex over (θ)}.sub.base(n) {circumflex over (θ)}.sub.base(n−1) {circumflex over (θ)}.sub.base (n−N)]  Equation V

[0050] The parameter vector is given in the following Equation VI:

w(n)=[w.sub.0 w.sub.1 w.sub.N]  Equation VI

[0051] The disturbance-free relative tilt angle is given in the following Equation VII:

θ.sub.relative(n)=θ.sub.actuator−w.sup.T(n)×(n)   Equation VII

[0052] The adaptive estimator for the filter parameters is given in the following Equation VIII:

w(n+1)=w(n)+μe(n)×(n)   Equation VIII

[0053] The desired actuator relative tilt angle is thus obtained as θ.sub.relative.

[0054] In some examples, sensor data processing module 112 (FIG. 1) performs a method of computing linear position using an accelerometer of an IMU 102, and a low-bandwidth laser sensor 104. In this method, as shown in FIG. 4B, the measured acceleration along the linear actuator (on the piston rod) is a.sub.x and the laser sensor measured low-bandwidth position is x.sub.lazer. Then, a high bandwidth drift-free estimate of linear position can be obtained using the following Equations IX and X:

{circumflex over ({dot over (x)})}={circumflex over (v)}+k.sub.1({circumflex over (x)}−x.sub.laser)   Equation IX

{circumflex over ({dot over (v)})}=a.sub.x+k.sub.2({circumflex over (x)}−x.sub.laser)   Equation X [0055] where: the gain vector [k.sub.1 k.sub.2].sup.T is chosen so as to stabilize the matrix

[00003]$\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]+\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]\left[10\right].$

[0056] In some examples, sensor data processing module 112 (FIG. 1) performs a method of computing end effector position from IMUs 102. In some examples, the method includes the following: (1) The vertical angle of each kinematic link on the machine is measured using an IMU 102, relative to ground (gravity direction); (2) the relative tilt angle between each link and the base is estimated as previously discussed above; and (3) as each kinematic link is of a known length, the end effector position can be recovered from the estimated angles using a forward kinematics equation.

[0057] As an example of this method, consider a construction vehicle that includes a vehicle base, and two rotating links (the method holds generally for m links) with an IMU 102 placed on each link. When the vehicle is on flat ground, all link rotation axes are horizontal. Let vectors p.sub.1 and p.sub.2 define the positions of points on the links of the machine. Each vector consists of the three coordinates x, y and z. Let each link have its own local coordinate system, such that p.sub.1 and p.sub.2 expressed in the link frame of reference, p.sub.1.sup.L and p.sub.2.sup.L are as follows, where l.sub.m is the length of the mth link:

p.sub.m.sup.link[l.sub.m0 0].sup.T   Equation XI

[0058] Define p.sub.E as the position vector of the end effector, relative to the joint between the base of the vehicle and the first link. Then p.sub.end can be expressed as:

p.sub.end=p.sub.1+p.sub.2   Equation XII

[0059] FIG. 7 is a diagram illustrating a vehicle with positions of such points on links and of an end effector according to one example. As shown in FIG. 7, the vehicle includes IMUs 102(3), 102(8), and 102(9).

[0060] To express p.sub.end in the base frame of reference, p.sub.end.sup.base, the position vector of each link is expressed in the base frame of reference, which can be accomplished with direction cosine matrices. Let R.sub.j.sup.k be the direction cosine matrix that will express a vector in the jth frame to its representation in the kth frame:

p.sup.k=R.sub.j.sup.kp.sup.l   Equation XIII

[0061] Thus, the end effector position in the machine coordinate system can be expressed as:

p.sub.end.sup.base=R.sub.ground.sup.baseR.sub.1.sup.groundp.sub.1.sup.link+R.sub.ground.sup.baseR.sub.2.sup.groundp.sub.2.sup.link   Equation XIV

[0062] As the method is not concerned with the horizontal plane (yaw) angle in this example, the ground frame of reference is defined such that the ground z-axis is aligned with gravity (pointing downwards), and the ground x-axis is coplanar with the ground z-axis and the base and link x-axes. That is, the ground y-axis will have zero x component when expressed in the base or link frame of reference:

[00004]$\begin{array}{cc}{R}_{\mathrm{ground}}^{\mathrm{base}}\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=R_{\mathrm{ground}}^{\mathrm{link}}\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}0\\ -\\ -\end{array}\right]& \mathrm{Equation}\mathrm{XV}\end{array}$

[0063] Equation XV implies that the (1,2) entries of and R.sub.ground.sup.base and R.sub.ground.sup.link are always zero. When using the terrain-induced slope and vibration technique described above, θ.sub.relative(n) is estimated for each link, which fully constrains the rotation matrix of each link relative to the base, R.sub.link.sup.base(n). Thus, the end effector position may be recovered as follows:

p.sub.end.sup.base(n)=R.sub.1.sup.base(n)p.sub.1.sup.link+R.sub.2.sup.base(n)p.sub.2.sup.link   Equation XVI

[0064] In the case when a link has a translational component as well as a rotational component, then the position vector in the link frame of reference is no longer a static value. Here, we may use the estimate of actuator linear position to estimate the link length, l(n), and we can write:

p.sub.m.sup.link(n)=[l.sub.m(n)0 0].sup.T   Equation XVII

[0065] Suppose in the two link example considered here that the second link has a translational component, then the end effector position can be estimated as:

p.sub.end.sup.base(n)=R.sub.1.sup.base(n)p.sub.1.sup.link+R.sub.2.sup.base(n)p.sub.2.sup.link(n)   Equation XVIII

[0066] In some examples, sensor data processing module 112 (FIG. 1) performs a method for preventing tip over of vehicles using end effector position estimation. FIG. 8 is a diagram illustrating a vehicle 800 with a tip-over prevention system according to one example. In some examples, the method includes the following three steps:

[0067] (1) The end effector position is estimated using a combination of IMUs 102(1)-102(3) and laser sensors 104 (FIG. 1). The IMUs 102(1)-102(3) help estimate rotational tilt angles of links and also help estimate linear translational motion in the case of equipment with telescoping links, as has been described above.

[0068] (2) The moment arm (tilt inducing moment or torque) of the load carried by the end effector 810 is estimated in real time as a function of the position of the end effector 810 and of the weight carried. The weight carried may be estimated from the actuator hydraulic pressure using hydraulic pressure sensor 808, or by any other means of determining an estimated load weight 806.

[0069] (3) The range of motion of the end effector 810 and the speed of travel of the vehicle 800 may be limited by system 100 (FIG. 1) to a safe operating limit 802, such that the weight carried by the end effector 810 will not cause the vehicle to tip over. For example, the range of motion could be limited by system 100 such that the height of the end effector 810 together with a knowledge of the estimated weight load 806 does not exceed a specified threshold (e.g., lift height limit 804). The speed and acceleration of the vehicle 800 may also be limited, since longitudinal acceleration causes load transfer from front to rear wheels, thus increasing the danger of tip over.

[0070] FIG. 9 is a flow diagram illustrating a method 900 for estimating position of an end effector according to one example. Gyroscope.sub.b 902 and accelerometer.sub.b 904 provide sensor data to base tilt estimation (inertial) unit 918. Gyroscope.sub.1 906 and accelerometer.sub.1 908 provide sensor data to link 1 tilt estimation (inertial) unit 920. Accelerometer.sub.1 908 and laser.sub.1 910 provide sensor data to link 1 length estimation unit 922. Gyroscope.sub.n 912 and accelerometer.sub.n 914 provide sensor data to link n tilt estimation (inertial) unit 924. Accelerometer.sub.n 914 and laser.sub.n 916 provide sensor data to link n length estimation unit 926. Note that laser and link length estimation steps may be present only on links with variable lengths (e.g., extension booms).

[0071] Based on the received sensor information, base tilt estimation (inertial) unit 918 determines base tilt values, θ.sub.base, which are provided to terrain vibration and slope compensation unit 928. Link 1 tilt estimation (inertial) unit 920 provides tilt values, θ.sub.1,inertial, to terrain vibration and slope compensation unit 928. Link n tilt estimation (inertial) unit 924 provides tilt values, θ.sub.n,inertial, to terrain vibration and slope compensation unit 928. Based on the received information, terrain vibration and slope compensation unit 928 determines tilt values, (θ.sub.1,relative, . . . , θ.sub.n,relative), which are provided to end effector position estimation unit 930.

[0072] Based on received sensor information, link 1 length estimation unit 922 determines a link length l.sub.1, which is provided to end effector position estimation unit 930. Based on received sensor information, link n length estimation unit 926 determines a link length l.sub.n, which is provided to end effector position estimation unit 930. Based on the information received from units 928, 922, and 926, end effector position estimation unit 930 determines an end effector position.

[0073] FIG. 10 is a flow diagram illustrating a method 1000 for tip-over prevention of a vehicle according to one example. End effector position estimation unit 930 receives information as shown in FIG. 9, and determines an end effector position, which is provided to payload weight estimation unit 1002. Hydraulic line pressure sensor (or other load sensing measurement) 1004 provides hydraulic line pressure measurements to payload weight estimation unit 1002. Based on information received from units 930 and 1004, payload weight estimation unit 1002 determines an estimated payload weight, which is provided to vehicle safe operating limits unit 1006 along with the estimated end effector position. Based on the received information, vehicle safe operating limits unit 1006 controls the vehicle to prevent a tip over event, which may include restricting speed, acceleration, lift height of the end effector, as well as other aspects of vehicle operation.

[0074] FIG. 11 is a diagram illustrating an excavator vehicle 1100 with three lengths and three tilt angles that may be used to estimate a position of the end effector according to one example. Excavator vehicle 1100 includes three hydraulic actuators 1102, 1106, and 1108 and two links 1104 and 1110 to move the end effector 1112, which is a bucket in this example. The length of the link 1104 is represented by l.sub.1. The length of the link 1110 is represented by l.sub.2. The length from the top of the end effector 1112 to the bottom of the end effector 1112 is represented by l.sub.3. The angle of link 1104 relative to horizontal is represented by θ.sub.1. The angle between link 1104 and link 1110 is represented by θ.sub.2. The angle between link 1110 and the end effector 1112 is represented by θ.sub.3. The position of the bottom of the end effector 1112 is the position to be estimated, and this position is represented by an arrow with a question mark. FIG. 12 is a diagram illustrating the excavator vehicle 1100 shown in

[0075] FIG. 11 with a position estimation system that measures three rotational motions to determine the end effector position according to one example. The position estimation system includes four IMUs 102, which are coupled together via a communication link 1202 for power and communications. In one example, the communication link 1202 includes a controller area network (CAN) bus that allows microcontrollers and other devices to communicate with each other without a host computer. A first IMU 102 is positioned near a top portion of the end effector 1112. A second IMU 102 is positioned on the link 1110. A third IMU 102 is positioned on the link 1104. A fourth IMU 102 is positioned on a base of the vehicle 1100.

[0076] The IMUs 102 may be used to estimate the angles θ.sub.1, θ.sub.2, and θ.sub.3 shown in FIG. 11, and this information, along with the known lengths l.sub.1, l.sub.2, and l.sub.3, may be used to estimate the position of the end effector 1112. The IMU 102 located on the base of the vehicle 1100 may be used to measure reference signals that can be used to compensate for terrain slope and terrain induced vibrations. Other elements of system 100 (FIG. 1), such as computing device 106, may also be positioned on or in vehicle 1100.

[0077] FIG. 13 is a diagram illustrating vibration and slope compensation according to one example. In some examples, sensor data processing module 112 (FIG. 1) performs a method of removing the influence of terrain-induced slope and vibrations on acceleration signals. The module 112 may use the accelerometers and gyroscopes of an IMU on the vehicle base to provide a reference signal related to upstream measurement of terrain-induced disturbances that influence the accelerometers and gyroscopes on the link. For this method, θ.sub.base the estimated base is tilt angle of the base, which, as shown in FIG. 13, is provided to an unknown transformation, G.sub.unknown, and to an adaptive M-Tap finite impulse response (FIR) filter. This variable retains the information of road slope and static angle of the base, but does not retain high frequency vibrations. The unknown transformation is an unknown transfer function that relates the disturbance measured at the base to the disturbance at each IMU location. Since the influence of the disturbance (due to terrain slope and terrain-induced vibrations) on the IMU sensor signal is to be subtracted, the method figures out how the disturbance measured on the base is related to the disturbance experienced at the IMU sensor location. The unknown transfer function may be estimated using a filtered-x least mean squares (FXLMS) method. The adaptive FIR filter estimates the unknown transfer function and enables the disturbance at the IMU sensor location, {circumflex over (θ)}.sub.disturb, to be estimated. The unknown transformation outputs θ.sub.disturb, and the adaptive FIR filter outputs {circumflex over (θ)}.sub.disturb. θ.sub.disturb, {circumflex over (θ)}.sub.disturb, and θ.sub.link,inertial are combined as shown in FIG. 13 to determine θ.sub.link,relative. θ.sub.link,inertial is the estimated inertial tilt angle of the link, and θ.sub.link,relative is the desired relative tilt angle of the link.

[0078] Some applications may involve the estimation of linear position of a structure (e.g., a telehandler arm). An IMU accelerometer may double integrated to obtain linear position changes, but the accelerometer does not determine absolute position, and an accelerometer may have drift when integrated. A laser time of flight sensor or other sensor may be used to measure absolute length, but may be lower bandwidth than an IMU. Some examples of the present disclosure combine an absolute position sensor, such as a laser time of flight sensor, with an IMU to provide inexpensive, accurate, and high bandwidth linear position estimation.

[0079] Some examples disclosed herein include a configuration of IMU sensors in order to be able to compute tilt angles of links on construction or other off-road vehicles. This configuration may include IMUs on individual links plus an IMU on the vehicle base. The inertial angles of the vehicle base and of the link may both be utilized to calculate relative angle of the link, thus removing the influence of terrain slope on tilt angle calculation. Some examples include a combination of an inexpensive laser sensor (e.g., ˜$5) and an inexpensive IMU (e.g., ˜$5) to compute linear position of a piston rod of a translational actuator. Some examples use an IMU on the vehicle base in order to obtain an upstream measurement of vibrations (induced by the road or by the engine), which is then utilized in an adaptive feedforward algorithm to cancel the effect of such vibrations on the IMUs located on the links. Examples disclosed herein include specific methods to calculate inertial tilt angles and to cancel the influence of vibrations. Some examples calculate end effector position using the sensors described herein; use the additional knowledge of load weight to compute real-time propensity of the vehicle to tip over; and prevent the tip-over from occurring by preventing dangerous motions of the vehicle links.

[0080] Some examples are directed to a method of using sensors together to create variables that could not be easily measured otherwise. Some examples provide a low-price alternative to other methods that may be used on agricultural and construction vehicles for end effector position determination.

[0081] Measurements of linear positions and angular positions of links on such actuators are currrently either not done at all, or may be done using more expensive sensors (e.g., over $500), which may require drilling a hole through the entire length of the piston rod in a hydraulic cylinder. Such measurements may also be done using potentiometers, which are contacting sensors and prone to frequent failure. Compared to the such systems, some examples disclosed herein are ten times less expensive, can be installed on a vehicle with very little effort, and include only non-contacting sensors that are robust in performance.

[0082] Although specific embodiments have been illustrated and described herein, it will be appreciated by those of ordinary skill in the art that a variety of alternate and/or equivalent implementations may be substituted for the specific embodiments shown and described without departing from the scope of the present invention. This application is intended to cover any adaptations or variations of the specific embodiments discussed herein. Therefore, it is intended that this invention be limited only by the claims and the equivalents thereof.