Beruflich Dokumente
Kultur Dokumente
59
The model has been presented in digging mode only, so for the proposed dynamic
model, only three degrees of freedom RRR (excluding the swing motion) are
considered (because while swinging, backhoe experiences an extra load and that is
payload in the bucket that does not cause large amount of torques at different joints
as compared to the digging task does, so the fourth degree of freedom is ignored).
The inertia tensor of the backhoe components is taken in a generalized form with
no assumption like the frame axes are not coinciding with the axes of center of
mass of the body, but it has been assumed that the link frame axes are coinciding
with the principle axes of the body to cause the products of inertia to be zero in an
inertia tensor.
. (4.1)
= i
dt i
i
. (4.2)
The left side of the dynamic equation (4.2) can be interpreted as sum of the torques
due to kinetic and potential energy present in the system. In the right hand side of the
equation (4.2), i is the joint torque for joint i that is provided by the actuator (i+1) in our
60
case. If i = 0, it means that joint i does not move and if i is not equal to zero, it means the
backhoe movement is modified by the actuator (i + 1) of joint i.
Now, the kinetic energy of the differential mass dmi of link i, for i = 2, 3 and 4 moving
with velocity 0vi (= vi) with respect to the base frame {0} is,
dK E i =
1
dmi i
2
. (4.3)
The trace operator (If A is a matrix of size n n, then trace operator Tr =
n
i=1 a ii )
is used to obtain i
as,
i 2 = Tr i i T
. (4.4)
. (4.5)
Note that here limits are taken from i = 2 to i = 4, because the dynamic model
presented is for the boom, arm and bucket linkage only (RRR configuration) and the
swing motion is not considered. Where, 0Tj-1 is a transformation matrix of the size 4 4
of frame {j-1} relative to the base frame {0}, j is a matrix when post multiplied with 0Tj1,
and pre multiplied with j-1Ti gives the partial derivative of the transformation matrix 0Ti
with respect to the corresponding joint angle i, and can be given by:
0 1 0
0 0
j = 1
0
0 0
0
0 0
0
0
0
0
The mass of link contributes inertia forces during motion of the link. The mass
properties, which reflect all the inertial loads with respect to rotations about the origin of
frame of interest, are presented by a moment of inertia tensor Ii. The moment of inertia
tensor Ii is a 4 4 symmetric matrix which; characterizes the distribution of mass of a
rigid body or link i, and it can be defined as:
1
2
i =
Ixy
1
2
Iyz
mi yi
61
Ixz
mi x i
Iyz
mi yi
mi zi
mi
. (4.6)
Where, Ixx, Iyy and Izz are moment of inertias of a body with respect to X, Y and Z
axes respectively, Ixy, Iyz and Ixz are the products of inertia and are taken as zero by
assuming that the axes of the reference frame are aligned with the principle axes of the
body, mi is the mass of link I, and i i = xi yi zi 1
defining the position of the centre of mass or centroid of the link i from the origin of the
link i of Oi . So, new inertia tensor for our case will now be modified as:
1
2
i =
mi x i
mi yi
0
1
2
0
mi yi
0
mi x i
. (4.7)
mi zi
mi
The total potential energy of backhoe mechanism is sum of the potential energy of
the links, i.e. boom, arm and bucket, then given by,
PE =
4
i=2 mi
T 0i i i
. (4.8)
The negative sign indicates that the work is done on the system to raise link i
against gravity. Where, g is the vector of acceleration due to gravity (m/s2):
= g x g y g z 0 = 0 9.81 0 0
T = gx gy gz 0
= 0 9.81 0 0
The acceleration due to gravity g is the 4 1 gravity vector with respect to base
frame O .
4
0
Tr[( j1 j
j1
i )i ( k1 k
k1
mi T 0i i i
i ) ]j k +
i=2
. (4.9)
According to the Lagrange-Euler dynamic formulation, the generalized torque i of
actuator i+1 at joint i, to drive link i of the backhoe is given by equation (4.2) as:
i =
d La
La
dt i
i
62
i =
Mij j +
j=2
hijk j k + Gi + load
j=2 k=2
For i = 2, 3, 4.
. (4.10)
Where,
4
Tr pj p T pi
Mij =
p=max (i,j)
. (4.11)
hijk =
2 k
j
i
. (4.12)
4
T
p
p=i mp pi p
Gi =
. (4.13)
Load Ft , Fn
. (4.14)
1
ij =
j1 j j1i
0
for j i
for j >
. (4.15)
ij
=
k
j1 j j1k1 k k1i
for i k j
k1 j k1j1 k j1Ti
for i j k
0
for i < <
. (4.16)
63
Note that equation (4.12) has been taken from the reference (W. Khalil et al., 2004).
Fig. 4.1 shows the resolution of the resistive force offered by the ground on the teeth of
the bucket. This resistive force can be resolved into the tangential and normal directions
on the bucket teeth as shown in Fig. 4.1, and the load vector given in equation (4.14) can
be found as given in (A. J. Koivo et al., 1996).
Equation (4.10) is the dynamic model of the backhoe in generalized form and gives
a set of 3 nonlinear, coupled, second order ordinary differential equations for 3 links of
the three degree of freedom backhoe. These equations are the equations of motion or the
dynamic equation of motions for the backhoe.
In equation (4.10), ij represents an inertia matrix. It is known as effective inertia
when acceleration of joint i causes a torque at joint i and coupling inertia when
acceleration at joint j causes a torque at joint i. The coefficient hijk represents the velocity
2
induced reaction torque at joint i. hijk j represents the centrifugal or centripetal force
acting at joint i due to velocity at joint j, and term hijk j k represents the Coriolis force
acting at joint i due to velocities at joint j and k. Gi is the gravity loading vector, and Fload
is the load vector acting on the bucket teeth due to the forceful and sudden interaction of
the bucket teeth with the ground and the equation (4.14) can be determined from the
64
geometry of the backhoe link mechanism which is explained in the section 4.4.5 and also
it can be found as given in (A. J. Koivo et al., 1996).
i-1
equations of motion. The final form of the equations of motion (or dynamic model) for
the backhoe can be given by:
i = + , + + Load (Ft , Fn )
. (4.17)
Tr p2 p Tp2
M22 =
p=max (2,2)
. (4.18)
Now from equation (4.15), let us first determine the term Tr 22 2 T22 , in this term let
us first determine d22 , as both i and j are equal (i =2 and j = 2) the first condition of
equation (4.15) can be applied to determine d22 as follows:
22
1
= 0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
22
0 1 0
0 0
1
0
0 0
0
0 0
S2
C2
=
0
0
0
0
0
0
C2
S2
0
0
0
0
0
0
C2 S2
S2
C2
0
0
0
0
a2 S2
a2 C2
0
0
0
0
1
0
a2 C2
a 2 S2
0
1
m2 x2
1
I Iyy 2 + Izz 2
2 xx 2
m2 y2
0
m2 x2
0
m2 y2
1
I + Iyy 2 Izz 2
2 xx 2
m2 z2
Tr 22 2 T22 = zz 2 + m2 2a2 x2 + a2 2
m2 z2
m2
. (4.19)
Similarly, the other two terms Tr 32 3 T32 and Tr 42 4 T42 can be determined
and given as follows:
Tr 32 3 T32 = IZZ 3 + m3 x3 2a3 + 2a2 c3 y3 2a2 s3 + a2 2 + a3 2 + 2a2 a3 c3
. (4.20)
Tr 42 4 T42 = IZZ 4 + m4 x4 2a4 + 2a2 c34 + 2a3 c4 y4 2a2 s34 + 2a3 s4 + a2 2 +
a3 2 + a4 2 + 2a2 a3 c3 + 2a3 a4 c4 + 2a2 a4 c34
. (4.21)
These equations (4.19), (4.20) and (4.21) when added together will form the first
element of the inertia matrix M22 as follows:
M22 = IZZ 2 + IZZ 3 + IZZ 4 + m2 2x2 a2 + a2
+ m3 x3 2a3 + 2a2 c3
M23
M33
M43
M24
M34
M44
All the other elements are given in section 4.4.6. In this section only the
methodology to derive the inertia matrix elements have been explained with the
illustration of the determination of the first element M22 .
4.4.2 Determination of the elements of a joint acceleration vector
The term represents the 3 1 joint acceleration vector, in which the acceleration of
joint 2, 2 can be determined by time derivative of the velocity of the joint 2, 2 of
equation (3.72).
2 =
VA 5 A 6 (A5 A6 )
A1 A5 (A1 A6 )sin( 1 2 2 )
. (3.72)
cos 1 2 2 2 VA 5 A 6 sin( 1 2 2 ) A 5 A 6
(A5 A6 )
A1 A5 (A1 A6 )
sin2 ( 1 2 2 )
. (4.23)
The time derivative of the equation (3.73) yields the acceleration of the joint 3, 3
of equation (3.73).
3 =
VA 7 A 8 (A7 A8 )
A2 A7 (A2 A8 )sin(3 1 2 3 )
. (3.73)
cos(3 1 2 3 ) 3 VA 7 A 8 sin(3 1 2 3 )A 7 A 8
(A7 A8 )
A2 A7 (A2 A8 )
sin2 (3 1 2 3 )
. (4.24)
By differentiating the equation (3.74) with respect to time yields the acceleration of
the actuator 5 piston in terms of the angle acceleration 1 and then 1 will be determined
in terms of the joint 4 angle acceleration 4 :
Therefore,
1 =
A9 A12
VA 9 A 10 (A9 A10 )
(A10 A12 )sin(2 1 1 )
. (3.74)
67
And
1 =
cos 2 1 1 1 VA 9 A 10 sin(2 1 1 )A 9 A 10
(A9 A10 )
A9 A12 (A10 A12 )
sin2 (2 1 1 )
. (4.25)
The following two relations are known to us from equations (3.75) and (3.76).
1 + = 4 3
2
. (3.75)
2 =
Now, substituting the equation (3.76) into (3.75) and rearranging the equation
(3.75) yields,
A3 A12 (A10 A12 )sin1
+ 3
A3 A11 (A10 A11 )sin2
4 = 1 1 +
. (4.26)
The acceleration of joint 4 becomes,
4 = 1
Hij =
hijk k
k=2
. (4.28)
68
Let us first determine the element H22 with the use of equation (4.28) and (4.12) as
follows:
H22 = h222 2 + h223 3 + h224 4
. (4.29)
Now let us first determine the term, h222 2 and for this h222 is to be determined
using equation (4.12) as follows:
h222 =
=
=0
2 2
2
2
2 2
This is because it is clear from equation (4.22) that the element M22 is not a
function of the joint 2 angle 2 , thus partial derivative of the element M22 with respect to
the joint 2 angle 2 will become zero. This leads to;
h222 2 = (0)2
. (4.30)
Now let us determine the second term, h223 3 of equation (4.29) and for this to be
determined h223 can be given as follows:
h223 =
=
2 3
2
2
2 3
h223 = m3 x3 a2 s3 + m3 y3 a2 c3 + m3 a2 a3 s3 + m4 x4 a2 s34 +
. (4.31)
. (4.32)
. (4.34)
So, the final velocity induced torque matrix or matrix of centripetal and Coriolis
torques can be given as:
H22
, = H32
H42
H23
H33
H43
69
H24
H34
H44
. (4.35)
All the other elements are given in section 4.4.6. In this section only the
methodology to derive the velocity induced torque matrix elements have been explained
with the illustration of the determination of the first element H22 .
4.4.4 Determination of the elements of a gravity torque vector
The gravity torque acting at joint 2, 3 and 4 is the function of joint displacement or joint
angle i for joints i = 2, 3 and 4, and can be collectively represented by a vector as follows:
G2
= G3
G4
. (4.36)
Using equation (4.13), let us first determine the first element of the gravity torque
vector, G2 as follows:
G2 = m2 22 22 + m3 32 33 + m4 42 44
Where, = g x g y g z 0 = [0 9.81 0 0] (assuming the acceleration due to gravity
is only acting in downward y direction), i i = xi yi zi 1
is a homogeneous coordinate
vector defining the position of the centre of mass or centroid of the link i from the origin
Oi of the link i. This leads to:
G2 = m2 g c2 x2 s2 y2 + a2 c2 + m3 g c23 x3 s23 y3 + a2 c2 + a3 c23 +
m4 g[c234 x4 s234 y4 + a2 c2 + a3 c23 + a4 c234 ]
. (4.37)
All the other elements are given in section 4.4.6. In this section only the
methodology to derive the gravity torque vector elements have been explained with the
illustration of the determination of the first element G2 .
4.4.5 Determination of the elements of a load vector
The loading vector resulting from the interactive forces due to soil tool interaction and its
elements are given in (A. J. Koivo et al., 1996) and can be given by:
Load Ft , Fn
F2
= F3
F4
. (4.38)
The elements F2, F3 and F4 of the loading vector are determined from the
geometrical Fig. 4.2, Fig. 4.3 and Fig. 4.4 respectively by considering the tangential and
normal components of the resistive force.
70
71
The element of load vector Load (Ft , Fn ) in equation (4.17) can be found from Fig.
4.2, Fig. 4.3 and Fig. 4.4, and also which are given in (A. J. Koivo et al., 1996) dynamic
model, as follows:
F2 = a2 [Ft sin 2 Fn cos(2 )]
F3 = a3 [Ft sin 23 Fn cos(23 )]
F4 = a4 [Ft sin + Fn cos]
Therefore, the load vector,
Load Ft , Fn
As shown in Fig. 4.1 the resistive force Fr is acting at the bucket teeth by making an
angle with the plane defined by the bottom plate of the bucket. The calculation
methodology and applications of this resistive force Fr has been thoroughly explained in
the chapter 6. The resistive force Fr offered by the soil on the bucket teeth is the function
of soil and tool properties as described in chapter 6. This force can be resolved in two
directions tangential and normal as shown in Fig. 4.1. While resolving Fr in tangential
direction will give the value of the tangential force Ft, and resolving Fr in the normal
direction will give the value of the normal force Fn as follows:
Ft = Fr cos()
And,
Fn = Fr sin()
72
But according to (A. J. Koivo et al., 1996), these two forces can be given by:
Ft = Fr cos(0.1) and Fn = Fr sin(0.1).
4.4.6 Determination of the final equations of motion
The final form of the equations of motion (or dynamic model) for the backhoe can be
given as follows:
i = + , + + Load (Ft , Fn )
. (4.40)
Where, the inertia matrix and its elements are given by:
M22
= M32
M42
M23
M33
M43
M24
M34
M44
+ m3 x3 2a3 + 2a2 c3
73
H23
H33
H43
H24
H34
H44
Where, the elements of the velocity induced torques are given as:
H22 = 0 2 m3 x3 a2 s3 + m3 y3 a2 c3 + m3 a2 a3 s3 + m4 x4 a2 s34 +
m4 y4 a2 c34 + m4 (a2 a3 s3 + a2 a4 s34 ) 3 m4 x4 a2 s34 + a3 s4 + m4 y4 a2 c34 +
a3 c4 + m4 (a3 a4 s4 + a2 a4 s34 ) 4 ,
H23 = m3 x3 a2 s3 + m3 y3 a2 c3 + m3 a2 a3 s3 + m4 x4 a2 s34 + m4 y4 a2 c34 +
m4 a2 a3 s3 + a2 a4 s34
2 m3 x3 a2 s3 + m3 y3 a2 c3 + m3 a2 a3 s3 +
3 m4 x4 a2 s34 + a3 s4 +
74
Where, the elements of the gravity torque vector are given as:
G2 = m2 g c2 x2 s2 y2 + a2 c2 + m3 g c23 x3 s23 y3 + a2 c2 + a3 c23 +
m4 g[c234 x4 s234 y4 + a2 c2 + a3 c23 + a4 c234 ] ,
G3 = m3 g c23 x3 s23 y3 + a3 c23 + m4 g[c234 x4 s234 y4 + a3 c23 + a4 c234 ] ,
and
4.5 Summary
The dynamic model of the three degrees of freedom mechanism of the backhoe excavator
using Lagrange-Euler formulation is established in section 4.4.6. The proposed dynamic
model can directly be utilized for designing the control algorithm for the backhoe
excavator for operating the backhoe excavator in an autonomous mode. The proposed
dynamic model has been compared with the standard three degrees of freedom excavator
dynamic model of (A. J. Koivo et al., 1996), also the results and validation of the
75
proposed dynamic model for the backhoe excavator have been discussed with the help of
the MATLAB codes (section B.2 of appendix B) for both the models in the chapter 10.
Thus chapters 3 and 4 have presented the complete foundation required to design
the parts of the backhoe, to operate the backhoe in autonomous mode. In the next chapter
5, the 3D model of the backhoe attachment has been presented using Autodesk Inventor
Professional 2011.
76