Estimation with gyros of the relative attitude between a vehicle body and an implement operably coupled to the vehicle body

An estimate of the relative attitude between an implement and a vehicle body is computed from a body angular velocity measurement received from at least one body gyro mounted on the vehicle body and from an implement angular velocity measurement received from at least one implement gyro mounted on the implement. A first system state vector estimate corresponding to a first time instant includes a representation of a first relative attitude estimate. An updated system state vector is computed based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement. A second system state vector estimate corresponding to a second time instant is predicted based at least in part on the updated system state vector and a time-dependent system model. The second system state vector estimate includes a representation of a second relative attitude estimate.

CROSS-REFERENCE TO RELATED APPLICATIONS

This application is a national stage (under 35 U.S.C. 371) of International Patent Application No. PCT/RU2014/000445, filed Jun. 23, 2014, which is herein incorporated by reference in its entirety.

BACKGROUND OF THE INVENTION

The present invention relates generally to control of an implement operably coupled to a body of a vehicle, and more particularly to the estimation, using gyros, of the attitude of the implement relative to the body of the vehicle.

In particular earthmoving operations, the attitude and position of an implement operably coupled to a vehicle body needs to be accurately controlled; consequently, the attitude and position of the implement needs to be accurately measured. In grading applications with a dozer, for example, the attitude and position of the dozer blade needs to be accurately controlled, and accurate measurements of the attitude and position of the dozer blade are needed. In some machine control systems, the attitude and position of the dozer blade are measured by sensors mounted on the dozer blade. The position of the dozer blade can be measured, for example, with a Global Navigation Satellite System (GNSS) receiver or a laser system. In these systems, a mast is installed on the dozer blade to support a GNSS antenna, a laser prism, or a laser receiver. The attitude of the dozer blade can be measured with two GNSS antennas, two laser prisms, or two laser receivers. Each GNSS antenna, laser prism, or laser receiver is supported by an individual mast installed on the dozer blade.

During earthmoving operations, the sensors are exposed to harsh environmental conditions, including high levels of shock and vibration, wide ranges of high and low temperatures, exposure to water, and impact with soil, stones, and rocks. Sensors mounted on a mast, in particular, are exposed and susceptible to damage.

BRIEF SUMMARY OF THE INVENTION

An implement is operably coupled to a vehicle body. In an embodiment of the invention, the relative attitude between the implement and the vehicle body is estimated. A first system state vector estimate is received. The first system state vector corresponds to a first time instant and includes a representation of a first relative attitude estimate corresponding to the first time instant. A body angular velocity measurement from at least one body gyro mounted on the vehicle body is received, and an implement angular velocity measurement from at least one implement gyro mounted on the implement is received. An updated system state vector is computed based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement. A second system state vector estimate is predicted. The second system state vector estimate is based at least in part on the updated system state vector and a time-dependent system model, corresponds to a second time instant, and includes a representation of a second relative attitude estimate corresponding to the second time instant.

DETAILED DESCRIPTION

In general, embodiments of the invention described below can be used for a vehicle including a vehicle body and an implement operably coupled to the vehicle body. An implement operably coupled to a vehicle body refers to an implement whose attitude relative to the vehicle body can be varied and controlled, either manually by an operator or automatically by a control system. In some vehicles, both the attitude and the position of the implement relative to the vehicle body can be varied and controlled.

Embodiments of the invention can be used, for example, for construction vehicles such as earthmoving machines (including dozers and motorgraders) and pavers: a dozer includes a dozer body and a dozer blade operably coupled to the dozer body; a motorgrader includes a motorgrader body (frame) and a motorgrader blade operably coupled to the motorgrader body; and a paver includes a paver body and a screed operably coupled to the paver body. In the discussions below, a dozer is used as a representative example of a vehicle for which embodiments of the invention can be used.

FIG. 1shows a schematic of a dozer100, which includes a dozer body102and a dozer blade104. The dozer body102includes a mainframe102F and a cabin102C, in which the operator sits. The dozer100travels across ground via a right track106R and a left track (not shown); left and right are viewed from the perspective of the operator sitting in the cabin102C. The dozer blade104is operably coupled to the dozer body102via support arms and hydraulic cylinders. The number of support arms and hydraulic cylinders varies with different dozer designs.FIG. 1shows support arm108as a representative support arm and shows hydraulic cylinder110, hydraulic cylinder112, and hydraulic cylinder114as representative hydraulic cylinders.

In general, both the attitude (angular orientation) and the position of the dozer blade104relative to the dozer body102can be controlled by controlling the extensions of the hydraulic cylinders. The hydraulic cylinders can be controlled manually by an operator (for example, via the joystick120) or automatically by a computer control system.

For grading operations, parameters such as the height of the dozer blade above the ground and the slope of the dozer blade relative to the ground are controlled. A system-state estimate system computes an estimate of the current system state and generates a feedback signal corresponding to the estimate of the current system state. In a manual control system, the feedback signal is inputted into a display system that displays the current values of the dozer blade parameters (such as the height and the slope of the dozer blade) on a lightbar or video display, and an operator manually adjusts the dozer blade to achieve and maintain the desired (target) values of the dozer blade parameters. In an automatic control system, the feedback signal is transformed into a control signal that is used by a hydraulic control system to automatically control the height and the slope of the dozer blade.

The geometrical configurations of dozers and the degrees of freedom of the dozer blade relative to the dozer body vary among different models of dozers. In the most general case, a dozer blade can have up to six degrees of freedom (three angular rotations varying the relative attitude between the dozer blade and the dozer body and three translations varying the relative position between the dozer blade and the dozer body). In most cases, a dozer is equipped with a 4-way blade or a 6-way blade. A 4-way blade has two degrees of freedom: lift and tilt. The lift is adjustable in two ways (up and down), and the tilt is adjustable in two ways (clockwise and counter-clockwise). A 6-way blade has three degrees of freedom: lift, tilt, and angle. The lift is adjustable in two ways (up and down), the tilt is adjustable in two ways (clockwise and counter-clockwise), and the angle is adjustable in two ways (left and right).

In practice, the number of parameters of the dozer blade to be controlled depends on the application. If the application requires control of only the slope of the dozer blade relative to the ground, then an estimate of the dozer blade attitude relative to the dozer body is sufficient. If the application requires control of both the slope of the dozer blade and the position of the dozer blade relative to the ground, then both an estimate of the dozer blade attitude relative to the dozer body and an estimate of the dozer blade position relative to the dozer body are needed.

If the dozer blade has no more than three degrees of freedom, an estimate of the dozer blade position relative to the dozer body can be calculated from the estimate of the dozer blade attitude relative to the dozer body. If the dozer blade has more than three degrees of freedom, additional measurements (such as the attitudes of support arms), along with the estimate of the dozer blade attitude relative to the dozer body, are needed to determine an estimate of the dozer blade position relative to the dozer body. Algorithms for calculating an estimate of the dozer blade position relative to the dozer body based on an estimate of the dozer blade attitude relative to the dozer body and based on geometrical parameters of the dozer are well-known in the art and are not described in further detail herein.

Values of the dozer blade attitude and the dozer blade position relative to the dozer body can then be used in combination with values of the dozer body attitude and the dozer body position relative to a local or geodetic coordinate system to calculate the values of the dozer blade attitude and the dozer blade position relative to the local or geodetic coordinate system. Refer toFIG. 1. An example of a local or geodetic coordinate system is the local navigation reference frame101. The Cartesian axes of the local navigation reference frame101are denoted ENU [East (xn)103, North (yn)105, Up (zn))107]. In common practice, the xn-ynplane is tangent to the World Geodetic System 1984 (WGS-84) Earth ellipsoid; however, various other orientations can be used.

Values of the dozer body attitude and the dozer body position relative to a local or geodetic coordinate system can be calculated from sensors such as global navigation satellite system (GNSS) antennas, laser prisms, and laser receivers mounted on the dozer body (for example, mounted on the roof of the cabin). Algorithms for calculating the values of the dozer blade attitude and the dozer blade position relative to the local or geodetic coordinate system, based on values of the dozer blade attitude and the dozer blade position relative to the dozer body in combination with values of the dozer body attitude and the dozer body position relative to a local or geodetic coordinate system, are well known to those skilled in the art and are not discussed in further detail herein.

Values of the dozer blade attitude and the dozer blade position relative to the local or geodetic coordinate system can then be used to generate a feedback signal in a manual blade control system or an automatic blade control system. Algorithms for generating a feedback signal from values of the dozer blade attitude and the dozer blade position relative to the local or geodetic coordinate system are well known to those skilled in the art and are not discussed in further detail herein. As discussed above, depending on the application, different parameters of the dozer blade can be controlled; the feedback signal depends on the parameters to be controlled.

PCT International Publication No. WO 2013/119140 (“Estimation of the Relative Attitude and Position between a Vehicle Body and an Implement Operably Coupled to the Vehicle Body”) describes a method for estimating the relative dozer blade attitude between the dozer blade and the dozer body using accelerometers or a combination of accelerometers and gyros.

In an embodiment of the invention described herein, an estimate of the current system state is computed based on measurements from gyros mounted on the dozer body and on the dozer blade. In an advantageous embodiment, the gyros are mounted in an inertial measurement unit (IMU) with a robust housing to withstand harsh environmental conditions. InFIG. 1, the inertial measurement unit IMU1140is mounted on the dozer body102, and the inertial measurement unit IMU2150is mounted on the rear of the dozer blade104. The IMU1140can be mounted in the cabin102C or on the mainframe102F. If the cabin102C has a suspension, then the IMU1140should be mounted on the mainframe102F to avoid spurious influences of cabin vibration to the IMU1140. In the embodiment shown, IMU1140and IMU2150each include three orthogonally-mounted gyros. In other embodiments, an IMU includes one gyro or two orthogonally-mounted gyros (depending on the number of angular degrees of freedom to be measured). The configurations of the IMUs can be the same or can be different.

Shown inFIG. 1are two Cartesian measurement reference frames: the body frame141and the blade frame151. The origin of the body frame141is placed at the origin of the IMU1140, and the measurement axes of the IMU1140are aligned with the axes of the body frame141. Similarly, the origin of the blade frame151is placed at the origin of the IMU2150, and the measurement axes of the IMU2150are aligned with the axes of the blade frame151.

The body frame141is fixed with respect to the dozer body102and is defined by three orthogonal axes (FIG. 2A): x1-axis221, y1-axis223, and z1-axis225. The x1-axis is directed along the roll axis of the dozer body102; the y1-axis is directed along the pitch axis of the dozer body102; and the z1-axis is directed along the yaw axis of the dozer body102. Each angle is measured counter-clockwise about the respective positive axis (right-hand rule): the rotation about the body x1-axis is the body roll angle ϕ1231; the rotation about the body y1-axis is the body pitch angle θ1233; and the rotation about the body z1-axis is the body yaw angle ψ1235. Gyros in the IMU1140measure components of the body angular velocity projected onto the x1, y1, and z1axes.

The blade frame151is fixed with respect to the dozer blade104and is defined by three orthogonal axes (FIG. 2B): x2-axis241, y2-axis243, and z2-axis245. The x2-axis is directed along the roll axis of the dozer blade104; the y2-axis is directed along the pitch axis of the dozer blade104; and the z2-axis is directed along the yaw axis of the dozer blade104. Each angle is measured counter-clockwise about the respective positive axis (right-hand rule): the rotation about the blade x2-axis is the blade roll angle ϕ2251; the rotation about the blade y2-axis is the blade pitch angle θ2253; and the rotation about the blade z2-axis is the blade yaw angle ψ2255. Gyros in the IMU2150measure components of the blade angular velocity projected onto the x2, y2, and z2axes.

Refer to the vector diagrams shown inFIG. 3AandFIG. 3B. InFIG. 3A, the dozer body102is rotating with an angular velocity ω1301. In the body frame, the angular velocity ω1is referenced as ω11and represented by

ω11=[ω1⁢x⁢⁢1ω1⁢y⁢⁢1ω1⁢z⁢⁢1],(E1)
where ω1x1, ω1y1, ω1z1are the components of ω1projected onto the x1, y1, z1axis, respectively. InFIG. 3B, the dozer blade104is rotating with respect to the dozer body102with a relative angular velocity ωr303. If the dozer body102is rotating with an angular velocity ω1301, then the dozer blade is rotating with an angular velocity ω2, where
ω2=ω1+ωr.  (E2)
In the blade frame, the angular velocity ω2is referenced as ω22and represented by

The dozer blade attitude relative to the dozer body can be represented in one of three following forms:

direction cosine matrix (DCM): nine direction cosines; and

Using the rotation quaternion q as the attitude representation, the dozer blade angular velocity vector, projected onto the axes of the body frame, is written as:
ω21=qω22q−1.  (E5)
The dozer blade angular velocity vector relative to the dozer body, projected onto the axes of the body frame, is written as:
ωr1=qω22q−1−ω11.  (E6)

A system can be described by a discrete time state space model:
k+1=ƒ(k),  (E7)
where:time is represented by discrete time epochs tk=t0+kΔt, where t0is an initial time, k is an integer, and Δt is the time interval between epochs (also referred to as the epoch duration);krepresents the system state vector at time epoch tk;k+1represents the system state vector at time epoch tk+1; andƒ represents a generalized system state function.

As discussed below, the extended Kalman filter procedure is used for calculating the estimate of the rotation quaternion. A brief summary of the extended Kalman filter procedure is first presented.

In the extended Kalman filter procedure, the following system equations are used for discrete time:
k+1=ƒ(k)+wk(E8)
k=h(k)+vk,  (E9)
where:krepresents the system state vector at time epoch tk;k+1represents the system state vector at time epoch tk+1;ƒ represents a generalized system state functionwkrepresents the process noise vector at time epoch tk;krepresents the measurement vector at time epoch tk;vkrepresents the measurement noise vector at time epoch tk; andh represents a generalized measurement function.

The process noise vector wkhas a covariance matrix Qk; and the measurement noise vector vkhas a covariance matrix Rk.

To estimate the system state vectorfrom the measurement vector, the following extended Kalman filter procedure is used. At time epoch tk, the Jacobian matrices Akand Hkare calculated. The [i, j]-th element of the matrix Ak(where i and j are integer indices) is given by

Ak⁡[i,j]=∂f⁡(xk)[i]∂xk⁡[j].(E10)
The [i, j]-th element of the matrix Hkis given by

The system state vector estimate is updated by a new measurement vectorkas follows:
Kk=PkHkT(HkPkHkT+Rk)−1(E12)
k=h(k)  (E13)
k=k+Kk(k−k)  (E14)
where:

Pkis the system state vector estimate covariance matrix at time epoch tk; and

Kkis the Kalman gain at time epoch tk.

The symbolkdenotes an estimate ofk.

The new system state vector estimate is then predicted using the system equations:
k+1=ƒ(f)  (E15)
Pk+1=AkPkAkT+Qk.  (E15)

The following extended Kalman filter procedure is used for calculating the estimate of the rotation quaternion {circumflex over (q)}. A state vector of the dozer system includes the rotation quaternion, the dozer body angular velocity, and the dozer blade angular velocity relative to the dozer body. The system state vector can include other components as well. In an embodiment, the system state vectorkcomprises the following components:

For the extended Kalman filter procedure, the initial estimate of the system state vector is required at the start up of the system. The system state vector can be initialized by the identity rotation quaternion, {circumflex over (q)}=[1,0,0,0]. The identity rotation quaternion corresponds to the axes of the blade frame151pointing along the same directions as the axes of the body frame141.

The discrete time state space equation for this system is given by (E8) above. Withkgiven by (E17), the components of (E8) are the following:
q0,k+1=q0,k−q1,kδx2,k−q2,kδy2,k−q3,kδz2,k+wq0,k(E21)
q1,k+1=q1,k−q0,kδx2,k−q3,kδy2,k−q2,kδz2,k+wq1,k(E22)
q2,k+1=q2,k−q3,kδx2,k−q0,kδy2,k−q1,kδz2,k+wq2,k(E23)
q3,k+1=q3,k−q2,kδx2,k−q1,kδy2,k−q0,kδz2,k+wq3,k(E24)
δx2,k+1=δx2,k+wδx2,k(E25)
δy2,k+1=δy2,k+wδy2,k(E26)
δz2,k+1=δz2,k+wδz2,k(E27)
ω1x2,k+1=ω1x2,k+wω1x2,k(E28)
ω1y2,k+1=ω1y2,k+wω1y2,k(E29)
ω1z2,k+1=ω1z2,k+wω1z2,k(E30)
where w(●),krepresents the component of wkcorresponding to the component(●),kofk.

The elements of the Jacobian matrix Akis calculated from (E10) as

In an embodiment, the body angular velocity and the blade angular velocity are measured. The system measurement equations for the body angular velocity and the blade angular velocity are described below.

The system measurement equation for the body angular velocity is given by
ω1,k=h1(k)+vk,  (E32)
where:ω1,kis the body angular velocity ω1at time epoch tk;vkis the measurement noise vector at time epoch tk; andh1is a generalized measurement function.
The components of (E32) are the following:
ω1x1,k=(q0,k2+q1,k2−q2,k2−q3,k2)ω1x2,k+2(q1,kq2,k−q0,kq3,k)ω1y2,k+2(q1,kq3,k+q0,kq2,k)ω1z2,k+v1x1,k(E33)
ω1y1,k=2(q1,kq2,k+q0,kq3,k)ω1x2,k+(q0,k2−q1,k2+q2,k2−q3,k2)ω1y2,k+2(q2,kq3,k−q0,kq1,k)ω1z2,k+v1y1,k(E34)
ω1z1,k=2(q1,kq3,k−q0,kq2,k)ω1x2,k+2(q2,kq3,k+q0,kq1,k)ω1y2,k+(q0,k2−q1,k2−q2,k2+q3,k2)ω1z2,k+v1z1,k(E35)
where:ω1x1,k, ω1y1,k, ω1z1,kare the components of the body angular velocity ω1,kprojected onto the x1, z1, z1axes, respectively, of the body frame at time epoch tk; andv1(●),kis the component of vkcorresponding to the component ω1(●),kof ω1,k.

The system measurement equation for the blade angular velocity is given by:
ω2,k=h2(k)+vk(E36)
where:ω2,kis the blade angular velocity ω2at time epoch tk;vkis the measurement noise vector at time epoch tk; andh2is a generalized measurement function.
The components of (E36) are the following:

ω2⁢x⁢⁢2,k=ω1⁢x⁢⁢2,k+2Δ⁢⁢t⁢δx⁢⁢2,k+vω⁢⁢2⁢x⁢⁢2,k(E37)ω2⁢y⁢⁢2,k=ω1⁢y⁢⁢2,k+2Δ⁢⁢t⁢δy⁢⁢2,k+vω⁢⁢2⁢y⁢⁢2,k(E38)ω2⁢z⁢⁢2,k=ω1⁢z⁢⁢2,k+2Δ⁢⁢t⁢δz⁢⁢2,k+vω⁢⁢2⁢z⁢⁢2,k,(E39)
where:ω2x2,k, ω2y2,k, ω2z2,kare the components of the blade angular velocity ω2,kprojected onto the x2, y2, z2axis, respectively, of the blade frame at time epoch tk; andvω2(●),kis the component of vkcorresponding to the component ω2(●),kof ω2,k.

To improve the accuracy of the system state vector estimate, additional information can be used to update the system state vector. In the quaternion representation, the quaternion norm is a known value, defined to be 1. The actual value of the quaternion norm, computed from the system state vector, will, in general, vary from 1 due to errors and noise. At each time epoch, the value of the quaternion norm can be considered to be a virtual measurement. The system measurement equation for the quaternion norm is then given by:
|qk|=1=h3(k)+vk,  (E40)
where:

|qk| is the quaternion norm at time epoch tk;

vkis the measurement noise vector at time epoch tk; and

h3is a generalized measurement function.

In component form, (E40) is written as:
|qk|=1=q0,k2+q1,k2+q2,k2+q3,k2+v|q|,k,  (E41)
where v|q|,krepresents the noise of the measurement of the rotation quaternion norm at time epoch tk.

As discussed above, the dozer blade attitude relative to the dozer body with, in general, three angular degrees of freedom can be represented by Euler angles (three parameters), a direction cosine matrix (nine parameters), or a quaternion (four parameters). For the Euler-angles representation, the number of parameters is equal to the number of degrees of freedom, and no normalization condition is required. For the direction-cosine-matrix representation and the quaternion representation, the number of parameters exceeds the number of degrees of freedom, and a normalization condition is required. As discussed above, for the quaternion representation, the normalization condition is q02+q12+q22+q32=1. For the direction-cosine-matrix representation, the normalization condition is C−1=CT, where C is the direction cosine matrix. Therefore, for the direction-cosine-matrix representation, the system state vector can also be updated with a virtual measurement based on the normalization condition.

In an embodiment, the system state vector is updated when the relative angular velocity of the dozer blade with respect to the dozer body is determined to be zero. The system measurement equation for the relative angular velocity of the dozer blade with respect to the dozer body is given by:
ωr,k=h4(k)+vk,  (E42)
where:ωr,kis the relative angular velocity of the blade with respect to the body at time epoch tk;vkis the measurement noise vector at time epoch tk; andh4is a generalized measurement function.
The components of (E42) are the following:

In an embodiment, the relative angular velocity of the dozer blade with respect to the dozer body is not directly measured. Measurements of control signals (see below), however, can determine whether the relative angular velocity of the dozer blade with respect to the dozer body is zero or non-zero. In the special case in which the relative angular velocity of the dozer blade with respect to the dozer body is zero, the following system measurement equations apply:

The measurement Jacobian matrices Hk, with elements

Hk⁡[i,j]=∂h⁡(xk)[i]∂xk⁡[j],(E11)
are calculated from the above measurement equations. For example, the measurement Jacobian matrix Hkfor the blade angular velocity measurement equation ω2,k=h2(k)+vkcan be written as:

Using the above equations, the extended Kalman filter procedure can be used to estimate the system state vector at each time epoch tk. The relative attitude between the dozer blade and the dozer body can be calculated from the rotation quaternion {circumflex over (q)}kcomponent of the system state vector estimatek. In some dozers, the blade supports provide a direct dependence between the blade relative attitude and the blade relative position. In these cases, the blade relative position can be calculated from the blade relative attitude and from known dozer geometrical parameters.

Embodiments of systems for estimating the relative attitude between the dozer blade and the dozer body are shown inFIG. 5AandFIG. 5B.

The hydraulic system controlling the extensions of the hydraulic cylinders uses mechanical valves or electric valves. A dozer operator can manually control the hydraulic cylinders via a joystick, such as the joystick120inFIG. 1. For controlling mechanical valves, the joystick can be coupled to a Cardan joint, and a mechanical assembly links the Cardan joint to the hydraulic valves. Movement of the joystick controls the hydraulic valves via the Cardan joint and the mechanical assembly. For control of electric valves, the joystick can be coupled to potentiometers. Movement of the joystick controls the settings of the potentiometers, which in turn controls the current or voltage to the solenoids driving the electric valves.

Refer to the embodiment shown inFIG. 5A, which uses a manual hydraulic control system. An operator provides manual input to the hydraulic control system510(which can be mechanical or electrical) via the joystick120. The hydraulic control system510controls the flow of hydraulic fluid to the hydraulic cylinders (such as the hydraulic cylinder110, the hydraulic cylinder112, and the hydraulic cylinder114shown inFIG. 1) and thereby controls the extensions of the hydraulic cylinders. The hydraulic cylinders are operably coupled to the dozer blade104and control the relative attitude and the relative position of the dozer blade104with respect to the dozer body102(FIG. 1).

Various network architectures and protocols can be used for the communications network502. Examples of the communications network502include a controller area network (CAN), an Ethernet network, and an Internet Protocol (IP) network. The IMU1140, which is mounted on the dozer body102, sends the signal541to the communications network502. The signal541includes measurements of the angular velocity of the dozer body (ω1x1, ω1y1, ω1z1). Similarly, the IMU2150, which is mounted on the dozer blade104, sends the signal551to the communications network502. The signal551includes measurements of the angular velocity of the dozer blade (ω2x2, ω2y2, ω2z2).

The rotational state of the dozer blade104(whether the relative angular velocity of the dozer blade with respect to the dozer body is zero or non-zero) can be determined by the system. For example, the joystick sensor506monitors the movement of the joystick120and sends the signal505to the communications network502. The signal505reports whether the relative angular velocity is zero or non-zero. If the joystick is not moving, then the dozer blade is not rotating relative to the dozer body and is also not translating relative to the dozer body. If the joystick is moving, then the response of the dozer blade is dependent on the control system. In some control systems, movement of the joystick causes only rotation of the dozer blade relative to the dozer body. In other control systems, movement of the joystick can cause rotation or translation (or both) of the dozer blade relative to the dozer body. In these control systems, the response of the dozer blade depends on the trajectory of the joystick (for example, front/back or left/right). The joystick sensor506then needs to distinguish movements of the joystick that cause the dozer blade to rotate or rotate and translate from movements of the joystick that cause the dozer blade to translate without rotating.

If the hydraulic control system510is a mechanical hydraulic control system, then the joystick sensor506can be mechanically coupled to the joystick120(for example, one or more potentiometers operably coupled to an electronic circuit). Non-contact sensors (for example, one or more optical or video sensors operably coupled to an electronic circuit) can also be used.

If the hydraulic control system510is an electrical hydraulic control system, a separate joystick sensor, as described above for a mechanical hydraulic control system, can also be used. In an electrical hydraulic control system, movement of the joystick controls the settings of the potentiometers, which in turn controls the current or voltage to the solenoids driving the electric valves. Therefore, the current or voltage to the solenoids can also be monitored to determine whether the relative angular velocity of the dozer blade with respect to the dozer body is zero or non-zero. The hydraulic control system510sends the signal507to the communications network502. The signal507, based on the current or voltage to the solenoids, reports whether the relative angular velocity is zero or non-zero.

For both mechanical and electrical hydraulic control systems, the operational state of the hydraulic cylinders can also be monitored by sensors in the hydraulic control system (for example, pressure sensors or flow sensors). The hydraulic control system510sends the signal509to the communications network502. The signal509, based on measurements by pressure or flow sensors, reports whether the relative angular velocity is zero or non-zero.

In general, signal505, signal507, and signal509can be used separately or in combination to monitor the relative angular velocity of the dozer blade with respect to the dozer body and to report whether the relative angular velocity of the dozer blade with respect to the dozer body is zero or non-zero.

The controller unit160sends the output signal510O to the communications network502and receives the input signal510I from the communications network502. The controller unit160receives the input signal503I from the user input/output devices504and sends the output signal503O to the user input/output devices504. Examples of the user input/output devices504include a keyboard, a touchscreen, a lightbar, and a video display.

The controller unit160receives the measurements of the angular velocity of the dozer body (ω1x1, ω1y1, ω1z1) from the IMU1140, the measurements of the angular velocity of the dozer blade (ω2x2, ω2y2, ω2z2) from the IMU2150, and status signals from the joystick sensor506and the hydraulic control system510. The controller unit160calculates an estimate of the relative attitude between the dozer blade and the dozer body, generates a feedback signal, and sends the feedback signal to the user input/output devices104. The feedback signal can be converted to a display driver signal that drives a display such as a lightbar or video display. The display displays the difference between the estimated value and the target value of the dozer blade attitude (and, in some instances, the difference between the estimated value and the target value of the dozer blade position).

Refer toFIG. 5B. The control system shown inFIG. 5Bis similar to the control system shown inFIG. 5A, except that the hydraulic control system is an electrical hydraulic control system520that can be automatically controlled by the controller unit160(in addition to being manually controlled by the joystick120). The electrical hydraulic control system520sends output signal521O to the communications network502and receives input signal521I from the communications network502. The output signal521O can report whether the relative angular velocity of the dozer blade with respect to the dozer body is zero or non-zero. The output signal521O can also report various control measurements. The controller unit160sends a feedback signal to the electrical hydraulic control system520. The electrical hydraulic control system520converts the feedback signal to a control signal that controls the drive voltage or current to the solenoids that drive the electric valves to control the dozer blade attitude (and, in some instances, the dozer blade position).

Each of the interfaces shown inFIG. 5AandFIG. 5Bcan operate over various communications media. Examples of communications media include wires, free-space optics, and electromagnetic waves (typically in the radiofrequency range and commonly referred to as a wireless interface).

An embodiment of the controller unit160is shown inFIG. 6. The controller unit160can be installed in the cabin102C (FIG. 1). The controller unit160can be configured, programmed, and operated by a user such as a control engineer, system installation engineer, or dozer operator; different users can be restricted to only a subset of functions. For example, a dozer operator could have restricted permission only to enter reference values of blade elevation and blade orientation; a control engineer or system installation engineer, however, could also have permission to enter control algorithms and setup parameters. One skilled in the art can construct the controller unit160from various combinations of hardware, firmware, and software. One skilled in the art can construct the controller unit160from various electronic components, including one or more general purpose processors (such as microprocessors), one or more digital signal processors, one or more application-specific integrated circuits (ASICs), and one or more field-programmable gate arrays (FPGAs).

The controller unit160includes a computer602, which includes a processor [referred to as a central processing unit (CPU)]604, memory606, and a data storage device608. The data storage device608includes at least one persistent, non-transitory, tangible computer readable medium, such as non-volatile semiconductor memory, a magnetic hard drive, or a compact disc read only memory.

The controller unit160further includes a communications network interface610, which interfaces the computer602with the communications network502, and a user input/output interface612, which interfaces the computer602with the user input/output devices504. Note that various input/output devices (not shown) can also communicate with the controller unit160via the communications network502. Data, including computer executable code, can be transferred to and from the computer602via a remote access terminal (not shown) communicating with the communications network502or via the user input/output devices504.

As is well known, a computer operates under control of computer software, which defines the overall operation of the computer and applications. The CPU604controls the overall operation of the computer and applications by executing computer program instructions that define the overall operation and applications. The computer program instructions can be stored in the data storage device608and loaded into the memory606when execution of the program instructions is desired. The algorithms shown schematically inFIG. 7-FIG. 10below can be defined by computer program instructions stored in the memory606or in the data storage device608(or in a combination of the memory606and the data storage device608) and controlled by the CPU604executing the computer program instructions. For example, the computer program instructions can be implemented as computer executable code, programmed by one skilled in the art, to perform algorithms. Accordingly, by executing the computer program instructions, the CPU604executes the algorithms shown schematically inFIG. 7-FIG. 10.

FIG. 7-FIG. 10show flowcharts summarizing methods, according to embodiments of the invention, for estimating the relative attitude of a dozer blade with respect to a dozer body. The methods can be performed, for example, by the controller unit160. The flowcharts show the processes for one time epoch; the processes are repeated at successive time epochs. In general, the IMU1140(FIG. 1) outputs measurements at discrete time instants relative to a reference clock in the IMU1140; the IMU2150outputs measurements at discrete time instants relative to a reference clock in the IMU2150; and the controller unit160processes measurements at discrete time instants relative to a reference clock in the controller unit160. The discrete time instants are commonly referred to as time epochs (or just epochs), and the time intervals between time epochs are referred to as epoch durations.

The epoch durations for the IMU1140, the IMU2150, and the controller unit160can be different or can be the same. The reference clocks in the IMU1140, the IMU2150, and the controller unit160can run asynchronously or can be synchronized to a common system time. If the reference clock in the IMU1140and the reference clock in the IMU2150are run asynchronously, then the sampling frequency of each IMU should be high enough such that the time difference between epochs from different IMUs are not more than a predetermined value defined by the required accuracy of the relative attitude; for example, a sampling frequency of 100 Hz should provide sufficient accuracy for most applications. In the embodiments shown inFIG. 7-FIG. 10, the steps are executed iteratively at each time epoch of the controller unit160. Other embodiments, however, can use other timing sequences.

Refer to embodiment shown inFIG. 7. In step702, the current time epoch tkstarts. In step704, the current system state vector estimatekis received; for example, it was calculated during the previous time epoch and stored in memory or on a data storage device. The current system state vector estimatekincludes, among its components, a representation of a current estimate of the relative attitude between the dozer blade104and the dozer body102.

The process then passes to step706, in which a body angular velocity measurement and a blade angular velocity measurement are received. The body angular velocity measurement, received from the IMU1140mounted on the dozer body102, is measured with respect to the body frame: (ω1x1, ω1y1, ω1z1). The blade angular velocity measurement, received from the IMU2150mounted on the dozer blade104, is measured with respect to the blade frame: ω2x2, ω2y2, ω2z2. Depending on the timing configuration, measurements from the IMU1140can be received before, after, or at the same time as, measurements from the IMU2150. As discussed above, IMU1140and IMU2150each include three orthogonally-mounted gyros. In general, each IMU can include one gyro, two orthogonally-mounted gyros, or three orthogonally-mounted gyros.

The process then passes to step708, in which the system state vector estimate is updated with the body angular velocity measurement and the blade angular velocity measurement. The updates can be performed separately or in combination. The process then passes to step710, in which the new system state vector estimatek+1is predicted. The new system state vector estimate {circumflex over (x)}k+1includes a representation of the new estimate of the relative attitude between the dozer blade and the dozer body. The process then passes to step712, in which the new estimate of the relative attitude between the dozer blade and the dozer body is extracted from the new system state vector estimate. In some embodiments, a new estimate of the relative position between the dozer blade and the dozer body is calculated from the new estimate of the relative attitude between the dozer blade and the dozer body and from known geometrical parameters of the dozer. The process then passes to step714, in which the current time epoch ends.

Refer to embodiment shown inFIG. 8. In step802, the current time epoch tkstarts. In step804, the current system state vector estimatekis received; for example, it was calculated during the previous time epoch and stored in memory or on a data storage device. The current system state vector estimatekincludes, among its components, a representation by a quaternion of a current estimate of the relative attitude between the dozer blade104and the dozer body102.

The process then passes to step806, in which a body angular velocity measurement, a blade angular velocity measurement, and an updated quaternion norm value are received. Details of the body angular velocity measurement and the blade angular velocity measurement are discussed above in reference toFIG. 7. The updated quaternion norm value is 1.

The process then passes to step808, in which the system state vector estimate is updated with the body angular velocity measurement, the blade angular velocity measurement, and the updated quaternion norm value. The updates can be performed separately or in various combinations. The process then passes to step810, in which the new system state vector estimatek+1is predicted. The new system state vector estimatek+1includes a representation by a quaternion of the new estimate of the relative attitude between the dozer blade and the dozer body. The process then passes to step812, in which the new estimate of the relative attitude between the dozer blade and the dozer body is extracted from the new system state vector estimate. In some embodiments, a new estimate of the relative position between the dozer blade and the dozer body is calculated from the new estimate of the relative attitude between the dozer blade and the dozer body and from known geometrical parameters of the dozer. The process then passes to step814, in which the current time epoch ends.

Refer to embodiment shown inFIG. 9. In step902, the current time epoch tkstarts. In step904, the current system state vector estimatekis received; for example, it was calculated during the previous time epoch and stored in memory or on a data storage device. The current system state vector estimatekincludes, among its components, a representation of a current estimate of the relative attitude between the dozer blade104and the dozer body102.

The process then passes to step906, in which a body angular velocity measurement and a blade angular velocity measurement are received. Details of the body angular velocity measurement and the blade angular velocity measurement are discussed above in reference toFIG. 7.

The process then passes to step908, in which the status of the blade relative angular velocity (relative angular velocity of the dozer blade with respect to the dozer body) is determined. The status of the blade relative angular velocity can be monitored and reported, for example, by signal505, signal507, or signal509(FIG. 5A). The process then passes to the decision step910. If the blade relative angular velocity is not zero, then the process passes to step912, in which the system state vector estimate is updated with the body angular velocity measurement and the blade angular velocity measurement. The updates can be performed separately or in combination.

Refer back to step910. If the blade relative angular velocity is zero, then the process passes to step914, in which the system state vector estimate is updated with the body angular velocity measurement, the blade angular velocity measurement, and the zero value of the blade relative angular velocity vector (ωr x2,k=0, ωr y2,k=0, ωr z2,k=0, where ωr(●),kare components of the blade relative angular velocity ωr,kin the blade frame). The updates can be performed separately or in various combinations.

The process then passes to step916, in which the new system state vector estimatek+1is predicted. The new system state vector estimatek+1includes a representation of the new estimate of the relative attitude between the dozer blade and the dozer body. The process then passes to step918, in which the new estimate of the relative attitude between the dozer blade and the dozer body is extracted from the new system state vector estimate. In some embodiments, a new estimate of the relative position between the dozer blade and the dozer body is calculated from the new estimate of the relative attitude between the dozer blade and the dozer body and from known geometrical parameters of the dozer. The process then passes to step920, in which the current time epoch ends.

Refer to embodiment shown inFIG. 10. In step1002, the current time epoch tkstarts. In step1004, the current system state vector estimatekis received; for example, it was calculated during the previous time epoch and stored in memory or on a data storage device. The current system state vector estimatekincludes, among its components, a representation by a quaternion of a current estimate of the relative attitude between the dozer blade104and the dozer body102.

The process then passes to step1006, in which a body angular velocity measurement, a blade angular velocity measurement, and an updated quaternion norm value are received. Details of the body angular velocity measurement and the blade angular velocity measurement are discussed above in reference toFIG. 7. The updated quaternion norm value is 1.

The process then passes to step1008, in which the status of the blade relative angular velocity (relative angular velocity of the dozer blade with respect to the dozer body) is determined. The status of the blade relative angular velocity can be monitored and reported, for example, by signal505, signal507, or signal509(FIG. 5A). The process then passes to the decision step1010. If the blade relative angular velocity is not zero, then the process passes to step1012, in which the system state vector estimate is updated with the body angular velocity measurement, the blade angular velocity measurement, and the updated quaternion norm value. The updates can be performed separately or in various combinations.

Refer back to step1010. If the blade relative angular velocity is zero, then the process passes to step1014, in which the system state vector estimate is updated with the body angular velocity measurement, the blade angular velocity measurement, the updated quaternion norm value, and the zero value of the blade relative angular velocity vector (ωr x2,k=0, ωr y2,k=0, ωr z2,k=0, where ωr(●),kare components of the blade relative angular velocity ωr,kin the blade frame). The updates can be performed separately or in various combinations.

The process then passes to step1016, in which the new system state vector estimatek+1is predicted. The new system state vector estimatek+1includes a representation by a quaternion of the new estimate of the relative attitude between the dozer blade and the dozer body. The process then passes to step1018, in which the new estimate of the relative attitude between the dozer blade and the dozer body is extracted from the new system state vector estimate. In some embodiments, a new estimate of the relative position between the dozer blade and the dozer body is calculated from the new estimate of the relative attitude between the dozer blade and the dozer body and from known geometrical parameters of the dozer. The process then passes to step1020, in which the current time epoch ends.

Specific embodiments of the invention were described above for a dozer including a dozer body and a dozer blade operably coupled to the dozer body. As discussed above, embodiments of the invention are generally applicable for a vehicle including a vehicle body and an implement operably coupled to the vehicle body.