12
e
13
e
14
e
15
e
11
12
13
14
=
15
=
O
( ) O P
1
L
13
B
11
B
12
B
14
B
15
B
moving platform
( , , )
p p p
x y z
16
e
16
0 =
a) i=1
x
y
z
x
y
z
1 i
e
2 i
e
3 i
e
4 i
e
5 i
e
6 i
e
1 i
2 i
3 i
4 i
5 i
6 i
O
( )
i i
O D
i
L
3
B
i
1
B
i
2
B
i
4
B
i
5
B
i
moving platform
A
i
b) i=2,3,4,5
Figure 3. General notations of the kinematic chain Li
Pentapod, the PKPM, is developed. The axes of the
revolute joints of all the kinematic chains are distributed
in three parallel lines, which are not coplanar and are
perpendicular to the axis of the spindle, as shown in Fig.
2(b) and Fig. 3. The axis
26
e
and
36
e
of the kinematic
chains L2 and L3 are collinear; the axis
46
e
and
56
e
of
the kinematic chains L4 and L5 are collinear; and the axis
15
e
+ +
+ +
= +
(1)
where K= P/2 P is the screw pitch,
( )
12 12
cos 0 90 , 90 l >
and ( )
11
90 , 90
Eq. (1) shows that , ,
p p p
x y z only affect
11
12
and
13
, and
11
and
12
rather than
13
will decide the
orientation of the ballscrew. The transformation matrix of
the moving platform relative to , ,
p p p
x y z , and
can be written as,
( , , , , )
0 1
s
p p p p
R P
R x y z
(
=
(
(2)
where
11 12
( ) ( ) ( ) ( )
s x y z x
R R R R R = ;
11
( )
x
R ,
12
( )
y
R , ( )
z
R and ( )
x
R denote the
rotation matrices of their corresponding rotation axis, and
P
, an operator v is
introduced to simplify the calculation
0
0
0
z y
z x
y x
v v
v v v
v v
(
(
=
(
(
So the angular velocity of the moving platform can be
expressed as,
T
p s s
R R =
(3)
where
11 12 11 11 12 12
11 12 11 12
T
11 11
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
2 2
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
2 2
( ) ( ) ( ) (
2 2
s x x y z x x y y z x
x y z z x x y z x x
x s x y x
R R R R R R R R R R R
R R R R R R R R R R
R R R R R
= +
+ +
= +
T
11 12
1 11 2 12 3 4
) ( ) ( ) ( ) ( )
2 2
s s x z x s x
R R R R R R R
C C C C
+ +
= + + +
The angular acceleration of the moving platform can be
written as,
T T
p s s s s
R R R R = +
(4)
where
T T
11 11 11 12
T T T
11 11 11 11 11
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
2 2 2 2
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
2 2 2 2 2
s x s x y x s s x z x s x
x x y y x x s s x y x s
R R R R R R R R R R R R R
R R R R R R R R R R R R
= + + +
   
+ + + +
 
\ . \ .
2
T T T
11
1 11 2 12 3 4 11 11 11 12 11
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
2 2 2 2 2
( ) ( )
2 2
s x x z z x x s s x z x
s x s s x s
R R R R R R R R R R R R
R R R R R R
C C C C C C
   
+ + + +
 
\ . \ .
   
+ + + +
 
\ . \ .
= + + + + +
12 13 11 14 11
22 12 12 23 12 24 12 33 34 44
11 12 s s s s
C C
C C C C C C
R R R R
+ +
+ + + + + +
+ + + +
5 Weiyang Lin, Bing Li, Xiaojun Yang and Dan Zhang: Modelling and Control
of Inverse Dynamics for a 5DOF Parallel Kinematic Polishing Machine
www.intechopen.com
( ) ( )
T
1 11 2 12 3 4 1 11 2 12 3 4
T T T T
1 1 11 11 1 2 11 12 1 3 11 1 4 11
T T T T
4 1 11 4 2 12 4 3 4 4
T
s s
R R C C C C C C C C
C C C C C C C C
C C C C C C C C
= + + + + + +
= + + +
+ +
+ + + +
According to Eq. (1) the velocity and the acceleration of
11
and
12
can be obtained,
11 2 2
z
z
y
y
p p
y z
=
+
(5)
( ) ( ) ( ) ( )
( )
3 2 2 3 2 2 2 2
11 2
2 2
2 2
z z y p z y y p z y p p z y p p
y z
y z y z y z
+ + + +
=
+
(6)
( )
( )
2 2 2 2
2
12
2
2
y z p x y p x z pz
x y z y z
x y z
+ +
=
+
+ + +
(7)
2 2 2
2 2 2
12
C C C C C C C C C
p p p p p p p p p
p p p
x p y p z p p p p x y p p x z p p y z p p
x y z
x y z x y z x y x z y z = + + + + + + + +
(8)
where
( ) ( )
2 3/ 2
2 2 2 2 2
C
den x y z y z
= + + +
( )
4 2 4 2 2 4 4 2 2 2 2 6 6
C 3 3 2 C
p
y z z x y z y x y x z y z d n x e
+ + + + + + =
( )
3 2 3 3 5 3 2 4
2 C C
p
x y z x y x y x y z x y z d y en
+ + + = +
( )
4 3 2 3 2 5 3 3
C 2 C
p
x z y x z y x z y x z x z d z en
+ = + + +
( ) 2
4 4 2 2
C 2 2 4 C
p
x y x z x y z den
x
+ + =
( ) 2
4 2 2 4 3 2
C 2 C
p
x y x y z x z x z den
y
= +
( ) 2
2 2 4 4 3 2
C 2 C
p
x y z x z x y x y den
z
+ =
( )
2 2 3 2 5 4 3 2
C 2 2 2 2 4 C
p p
x y y z x y x y y z y z den
= +
( )
2 2 3 2 3 2 5 4
C 2 2 4 2 2 C
p p
x z z y x z x z y z z y den
= +
3 3 3
C 2 6 6 C
p p
y z x y z x y z x y z den
= + +
Combining Eq. (3), Eq. (5) and Eq. (7), the angular velocity
of the moving platform can be expressed as,
p p
W q =
(9)
The linear and angular velocity of the moving platform
can be written as,
T
p
p p p
p
v
N x y z N q
= =
(10)
where
3 3 3 3
6 5
0
[ ]
p
I
N
W
=
,
T
T
p
q v =
.
The angular acceleration
p
O
1
A
1
O (P,D )
L
i
1
L
bi
r
Figure 4. Geometric model of the ith kinematic chain
6 Int. j. adv. robot. syst., 2013, Vol. 10, 314:2013 www.intechopen.com
Fig. 4 is a diagram of the kinematic chain Li (i=2, 3, 4 or 5)
for the closure vector modelling with the centre kinematic
chain L1. The closure equation of the ith kinematic chain
is
OA A D OP PD ( 2, 3, 4, 5)
i i i i
i + = + =
(12)
Let
4
A D
i i i i
l e =
,
PD
i bi
r =
, where
4 i
e
is the unit
vector along
D A
i i
.
The firstorder derivative of Eq. (12) with respect to t can
be written as,
( )
4 4 4 i i i i i p p bi
l e l e v r = +
(13)
In Eq. (13)
4 i
is the
extension speed of the ith kinematic chain;
p
v
is the
velocity of point P fixed on the moving platform relative
to the global coordinate system;
p
is the angular
velocity of the moving platform relative to the global
coordinate system. The angular velocity
4 i
can be
expressed as,
4 4
p
i p i
= +
(14)
where
4
p
i
and
6 i
e
and
6 i
e
, substituting
Eq. (14) to Eq. (13), we can obtain,
( ) ( )
4 4 4 4
p
i i i i i p p bi i i
l e l e v r l e = + +
(15)
Take the inner product with
4 i
e
(16)
Let
( )
4 4 i i i p p bi i i
l e v r l e = +
, we have
( )
4 4
p
i i i i
l e =
(17)
where
4
p
i
, and
4
p
i
lies on the plane spanned by
5 i
e
and
6 i
e
, so that
4
p
i
is perpendicular to vector
56 5 6 i i i
e e e =
, so the unit
direction vector
4
p
i
n
of
4
p
i
is
( )
4 4 56
p p
i i i i
K n K e = =
(18)
where K is a scalar, substituting Eq. (18) to Eq. (17)
( )
( )
56 4 i i i i i
K e l e =
(19)
Eq. (17) implies
4
0
i i
e =
can be written as,
( )
56
4 56
i i
i i i
e
K
l e e
=
(21)
Combining Eq. (18) and Eq. (21), the vector
4
p
i
is
obtained,
( ) [ ]
56 5 6
4 4
4 56 4 5 6
, ,
p p i i i i i
i i
i i i i i i i
e e e
K n
l e e l e e e
= = =
(22)
[ , , ] denotes a mixed product of three vectors, the
angular velocity of the ballscrew in the global coordinate
system can be written as,
[ ]
5 6
4 4
4 5 6
, ,
p i i i
i p i p
i i i i
e e
l e e e
= + = +
(23)
In order to calculate the angular velocities, the affine
projection method is introduced to solve the affine
coordinates.
4 i
(24)
The angular velocity
1 i
,
2 i
,
3 i
, and
5 i
can be
expressed as follows,
( )
( )
1 1 1
2 4 3 4
3 4 3
5 4 6 6
i i i
i i i i
i i i i
p
i p i i i
e
e
l e k
e e
=
= +
= +
(25)
3.4 Acceleration analysis of the kinematic model
Take the firstorder derivative of Eq. (13) with respect to t,
we obtain,
( ) ( )
( )
4 4 4 4 4 4 4
2
i i i i i i i i i i i
p p bi p p bi
l e l e l e l e
a r r
+ =
= + +
(26)
where
4 i
and
6 i
e
(29)
Let
( )
( ) ( )
4 4 4 4 4
4
2
i i i i i i i i i
p p bi i i p p bi
l e l e l e
a r l e r
= +
+
,
Eq. (29) can be written as,
( )
4 4
p
i i i i
l e =
(30)
where
4
p
i
is perpendicular to vector
i
, and
4
p
i
is
on the plane spanned by
5 i
e
and
6 i
e
, then
4
p
i
is
perpendicular to vector
56 5 6 i i i
e e e =
, so the unit
direction vector
4
p
i
of
4
p
i
is
( )
4 4 56
p p
i i i i
K K e = =
(31)
Substituting Eq. (31) to Eq. (30)
( )
( )
56 4 i i i i i
K e l e =
(32)
Simplifying Eq. (32) we have
( )
56
4 56
i i
i i i
e
K
l e e
=
(33)
The relative angular acceleration
4
p
i
of the ballscrew
4
B
i
is
[ ]
5 6
4 4
4 5 6
, ,
p p i i i
i i
i i i i
e e
K
l e e e
= =
(34)
We have the angular acceleration of the ballscrew,
[ ]
5 6
4 4
4 5 6
, ,
p i i i
i p i p
i i i i
e e
l e e e
= + = +
(35)
In order to calculate the angular velocities, the affine
projection method is used to solve the affine coordinates.
4 i
(36)
The angular acceleration
1 i
,
2 i
,
3 i
and
5 i
are
( )
1 1 1
2 4 3 4
3 4 3
5 4 6 6
i i i
i i i i
i i i i
p
i p i i i
e
e
l e k
e e
=
= +
= +
(37)
3.5 Jacobian matrix and the workspace analysis
3 i
s
i
, the scale of
3 i
s
i i i i
e J q (38)
with
( )
[ ]
( )
[ ]
[ ]
T
56 4 4 3 3
T T
3 4 4
4 5 6
T
56 4 4 4
T
3 3 4 4 6 5
4 5 6
,
, ,
, ,
i i i
i i i i
i i i i
i bi i i i i bi
i i bi
i i i i
e e e I
J e ke e
l e e e
e r l e e e r
I ke e r N
l e e e
(
+
( + +
(
(39)
then the kinematic Jacobian matrix can be obtained
[ ]
T
1 2 3 4 5
, , , , J J J J J J = (40)
Table 1 contains the basic geometric parameters of the
PKPM, Di and Ei6 are the local coordinates in the moving
platform.
Let ( ) J denote the condition number of the Jacobian
matrix, the dexterous workspace can be searched with the
limitation ( ) J <20 by the bound workspace search
method, the dexterous workspace of the yaw angle [0,
90] and roll angle [25, 25] is shown in Fig. 5, where Fig.
5(a) is the top view of the dexterous workspace and Fig.
5(b) is the oblique view.
The volume of the dexterous workspace of the PKPM is
0.7 x 0.4 x 0.3 m
3
, in the dexterous workspace the yaw
angle of the moving platform can rotate from 0 to 90,
which means the PKPM is suitable for a large workspace
fiveaxis machine tool.
A1 =(0.0,0.687,0.223) D1 =(0,0,0) E11 =(1.0,0,0) E16 =(1.0,0,0)
A2=(0.334,0.434,0.223) D2=(0.088,0.16,0.02) E21=(0.8192,0.5736,0) E26=(0.8192,0.5736,0)
A3=(0.334,0.434,0.223) D3=(0.088,0.16,0.02) E31=(0.8192,0.5736,0) E36=(0.8192,0.5736,0)
A4=(0.486,0.306,0.223) D4=(0.163,0.06,0.105) E41=(0.7071,0.7071,0) E46=(0.7071,0.7071,0)
A5=(0.486,0.30,0.223) D5=(0.163,0.06,0.105) E51=(0.7071,0.7071,0) E56=(0.7071,0.7071,0)
Table 1. Basic geometric parameters
(a) (b)
Figure 5. Dexterous workspace of the PKPM
9 Weiyang Lin, Bing Li, Xiaojun Yang and Dan Zhang: Modelling and Control
of Inverse Dynamics for a 5DOF Parallel Kinematic Polishing Machine
www.intechopen.com
(a) (b)
Figure 6. Workspace of the PKPM with zero yaw and roll angles
Fig. 6(a) is the top view of the workspace of the PKPM
obtained by fixing the yaw and roll angle of the moving
platform to zero, and Fig. 6(b) is the oblique view. As Fig.
6(a)(b) shows, the volume of the workspace of the PKPM
is 1.0 x 1.2 x 0.65 m
3
, it is larger than that (0.8x0.8x0.5 m
3
)
of the METROM Pentapod.
4. Inverse dynamics based on Kane's equation
4.1 The Kanes equation method
The Lagranges equation and the NewtonEuler method
are the most common and popular dynamic modelling
methods in robotics. They are very efficient and
convenient for dynamic systems with low degrees of
freedom and few components. The NewtonEuler method
requires an analysis of each component of the robot and
needs to take the internal forces into account. Finally, it
combines the dynamic equations of all the components to
obtain the final dynamic equations. So it needs much
more work in order to deal with lots of internal forces to
obtain the final dynamic equations. The Lagranges
equation based on the energy function does not need to
take the internal forces between the components of the
robot into account, but it requires that we calculate the
secondorder derivatives of the Lagrange function.
Therefore, the Lagranges equation is low in efficiency for
a system with high DOFs and lots of components.
The Kanes equation method retains the advantages of the
vector analysis method and the energy method, and also
avoids the drawbacks of Lagranges equation and the
NewtonEuler method. The efficiency of the algorithm
based on Kanes method reflects the high efficiency of the
modelling and the simple expression of the final dynamic
formula. The Kanes equation method does not care about
the internal force, so its dynamic modelling is more
efficient than the NewtonEuler method. Compared to the
Lagranges equation method, the equations of the
dynamic formula of the PKPM based on Kanes equation
method are expressed directly in the linear simple matrix
form, it can be calculated highly efficiently and can easily
be programmed for control.
4.1 Velocity and acceleration of the PKPM components
The mass centre of the body is represented by C; each
body mass centre of the parallel mechanism is essentially
coincident with the geometric centre, so the geometric
centre is regarded as the mass centre of the body. As the
motion of the component
1
B
i
is a fixedpoint rotation
motion through their centres of mass, the linear velocity
1 Ci
v
(41)
The mass centre acceleration of the component
3
B
i
is
( ) ( )
2 2 2 4 4 4 4 4 Ci Ci Ci i i i i i
a v e e = = +
(42)
The mass centre of component
3
B
i
is on the axis of the
ballscrew, suppose its eccentric vector is
3 3 4 Ci Ci i
r e =
,
then the mass centre velocity of component
3
B
i
is
( )
3 3 3 4 4 Ci Ci Ci i i
v r e = =
(43)
The mass centre acceleration of the component
3
B
i
is
( ) ( )
3 3 3 4 4 4 4 4 Ci Ci Ci i i i i i
a v e e = = +
(44)
10 Int. j. adv. robot. syst., 2013, Vol. 10, 314:2013 www.intechopen.com
Supposing the total length of the ballscrew is L, the mass
centre vector of the component
4
B
i
is
( )
4 4
2
Ci i i
r L l e =
, the mass centre velocity of the
component
4
B
i
is
( )
4 4 4 4
2
Ci i i i i i
v l e L l e = +
(45)
By taking the firstorder derivative of Eq. (45) with respect
to time t, the mass centre acceleration is
( ) ( )
4 4 4 4
4 4 4 4 4
2
2
Ci i i i i i
i i i i i i
a l e l e
L l e e
= +
+ + (
(46)
Similarly, the mass centre velocity and acceleration of the
component t
5
B
i
are
5 5 5 5 4 4 4 Ci Ci i i i i i i i
v e l e l e =
(47)
( ) ( )
( )
5 5 5 5 5 5 5 4
4 4 4 4 4 4 4
2
Ci Ci i i i i i i i
i i i i i i i i i
a e e l e
l e l e e
= +
+ (
(48)
In order to obtain the partial velocity of Kanes equation,
the angular velocity of each component and the linear
velocity of the mass centre relative to the generalized
velocity
T
p p p
q x y z ( =
can be obtained.
( )
( )
[ ]
( )
[ ]
[ ]
( )
T T
T T T
56 4 4 3 3 56 4 4 4
1 1 1 1 4 4
1 3 3 6 5 2
T
4 5 6 4 5 6
1 4
T
T T T
56 4 4
4 4 4 4 1 1
2 3 3 2
T
1 4
,
, , , ,
1
1
i i i i bi i i i i bi
i i i i i i
i
i i i i i i i i
i i
i i i
i i i i i i
i
i i
e e e I e r l e e e r
e e e e e e
I N q
l e e e l e e e
e e
e e e I
e e e e e e
I
e e
(
+
( = +
(
 

=

\ .
( )
[ ]
( )
[ ]
[ ]
( )
[ ]
( )
[ ]
T
3 3 56 4 4 4
3 3 6 5
4 5 6 4 5 6
T T
56 4 4 3 3 56 4 4 4
T T
3 4 4 3 3 4 4
4 5 6 4 5 6
,
, , , ,
,
, , , ,
i bi i i i i bi
i i i i i i i i
i i i i bi i i i i bi
i i i i i bi
i i i i i i i i
e r l e e e r
I N q
l e e e l e e e
e e e I e r l e e e r
ke e I ke e r
l e e e l e e e
(
+
( +
(
(
+
= + +
[ ]
( )
[ ]
( )
[ ]
[ ]
( )
[ ]
( )
6 5
T T
56 4 4 3 3 56 4 4 4
4 3 3 6 5
4 5 6 4 5 6
T T T
6 6 56 4 4 3 3 6 6 56 4 4 4
5
4 5 6
,
, , , ,
,
, ,
i i i i bi i i i i bi
i
i i i i i i i i
T
i i i i i i i i bi i i i i bi
i
i i i i i
N q
e e e I e r l e e e r
I N q
l e e e l e e e
e e e e e I e e e r l e e e r
l e e e l e
(
(
(
+
( = +
(
+
=
[ ]
[ ]
3 3 6 5
4 5 6
, ,
i i i
I N q
e e
( +
(
(49)
( )
[ ]
( )
[ ]
[ ]
( )
[ ]
2
T T
4 56 4 4 3 3 56 4 4 4
2 2 2 4 3 3 6 5
4 5 6 4 5 6
T
4 56 4 4 3 3
3 3 3 4
4 5 6
0
,
, , , ,
,
, ,
Ci
i i i i i bi i i i i bi
Ci Ci Ci i
i i i i i i i i
i i i i i
Ci Ci Ci i
i i i i
v
e e e e I e r l e e e r
v e I N q
l e e e l e e e
e e e e I e
v e
l e e e
=
(  
+
(  = +

(
\ .
( )
[ ]
[ ]
( )
( )
[ ]
( )
( )
[ ]
T
56 4 4 4
3 3 6 5
4 5 6
T T
4 56 4 4 3 3 56 4 4 4
T T
4 4 4 4 4 4 3 3
4 5 6 4 5 6
, ,
2 , 2
, , , ,
bi i i i i bi
i i i i
i i i i i bi i i i i bi
Ci i i i i i bi i i
i i i i i i i i
r l e e e r
I N q
l e e e
e e e e I e r l e e e r
v e e L l e e r L l e I
l e e e l e e e
(  
+
(  +

(
\ .

+
= +
\
[ ]
( ) ( )
[ ]
( )( )
[ ]
[ ]
6 5
T T T T
4 5 5 6 6 56 4 4 3 3 4 56 5 5 6 6 4 4 4
T T
5 4 4 4 4 4 5 5 6 5
4 5 6 4 5 6
,
, , , ,
i i Ci i i i i i i i i i Ci i i i bi i i i i bi
Ci i i i i bi i i Ci i
i i i i i i i i
N q
l e e e e e e e I l e e e e e r l e e e r
v e e e e r l e e N
l e e e l e e e
( 
( 

(
.
(
+
( = + + +
(
(50)
To simplify the formula expression, we rewrite Eq.
(49) and Eq. (50) as Eq.(51):
( )
( )
, 1,..., 5
=
ij ij
Cij Cij
W q q
i j
v V q q
(51)
Similarly, we can combine Eq. (35), Eq. (37), Eq. (42), Eq.
(44), Eq. (46) and Eq. (48) into the matrix form:
( ) ( )
( ) ( )
,
, 1,..., 5
,
= +
= +
ij ij ij
Cij aij aij
M q q C q q q
i j
a M q q C q q q
(52)
11 Weiyang Lin, Bing Li, Xiaojun Yang and Dan Zhang: Modelling and Control
of Inverse Dynamics for a 5DOF Parallel Kinematic Polishing Machine
www.intechopen.com
The coefficient matrices
ij
W ,
Cij
V ,
ij
M
,
aij
M depend
on the generalized variables q
, and
ij
C
,
aij
C depend
on both q
and q
.
4.2 Force analysis
(1) Active force and inertia force of the moving platform
The cutting force and moment and gravity act on the
moving platform in the polishing process, so the principal
force of the moving platform is,
t
p p p
F F m g = +
(53)
where
t
p
F
,
p
,
p
=
+
f
p ij
ij
f
ij ij
F F i and j
F
m g F otherwise
(56)
1 5
3
+ = =
= + =
f
p ij
f
ij i ij ij
f
ij
M M i and j
M M e M j
M otherwise
(57)
where
ij
m is the mass of B
ij
,
i
M is a driving moment
of the hollowshaft motor in the ith kinematic chain,
f
ij
F
and
f
ij
M
acting upon component B
ij
can be expressed as
* *
, [ ( )]
ij ij Cij ij ij ij ij ij ij
F m a M I I = = +
(58)
where
ij
I is the inertia matrix of the jth component in
the ith kinematic chain in the global coordinate system;
Cij
a
ij
ij
and
( ) s
ij
5,5 5,5
( ) ( )
1, 1 1, 1
( 1, 2, 3, 4, 5)
s s
s ij ij ij ij
i j i j
F F v M s
= = = =
= + =
(59)
5,5 5,5
* * ( ) * ( )
1, 1 1, 1
( 1, 2, 3, 4, 5)
s s
s ij ij ij ij
i j i j
F F v M s
= = = =
= + =
(60)
12 Int. j. adv. robot. syst., 2013, Vol. 10, 314:2013 www.intechopen.com
ij
W
and
Cij
V
denote the partial velocity matrix of
component
B
ij
s linear velocity and angular velocity
relative to the generalized variables
, , , ,
p p p
x y z
,
which is shown in Eq. (51). The partial velocities
( ) s
ij
v
and
( ) s
ij
and
( ) s
ij
, we can derive
the following dynamic equations about the PKPM
( ) ( ) ( ) ( ) ,
drv drv
J T M q q C q q q F q G q = + + +
(62)
where
T
p p p
q x y z =
the
velocitydependent frictional generalized force vector,
and ( ) G q
sim
stands for the angular velocity and
sim
r refers to
the radius.
The simulation results of the angular velocities and the
driving moments of the motors are shown in Fig. 7
The radius and angular velocity of the circular motion is
set at 0.3 m =
sim
r and 1rad/s =
sim
, so that the
linear velocity is 0.3m/s
sim sim
r = and the linear
acceleration is
2 2
0.3m/s =
sim sim
r . The maximum
angular velocities of the motors have reached 200 rad/s,
but the derived driving moments are less than the
maximum moment of the motor of 6.5 Nm. It shows that
the polishing feedrate of the PKPM could reach 18m/min.
We can also see that the driving characteristic of the
PKPM is quite good as it only needs relatively small
driving moments to drive.
Body Mass(Kg) Inertia (Kg*m
2
) Eccentricity (m)
1
B ( 1...5)
i
i = 6.72 diag(3.89, 5.03, 8.53)*0.01 0
2
B ( 1...5)
i
i = 8.07 diag(2.77, 2.21, 2.42)*0.01 0
3
B ( 1...5)
i
i = 3.84 diag(1.03,1.03,0.38)*0.01 0.08
4
B ( 1...5)
i
i = 19.42 diag(3.44,3.44 .00561) 0
5
B ( 2...5)
i
i = 1.34 diag([1.05, 1.37, 2.09)*0.001 0
15
B 37.22 diag(1.55, 2.0, 0.628) *0.1 0.135
Table 2. Property parameters of each component
.
13 Weiyang Lin, Bing Li, Xiaojun Yang and Dan Zhang: Modelling and Control
of Inverse Dynamics for a 5DOF Parallel Kinematic Polishing Machine
www.intechopen.com
(a) angular velocities of the motors
(b) driving moment of the motors
Figure 7. Simulation results ( 18 m/min
p
v = ,
2
0.3 m/s
p
a =
)
(a) angular velocities of the motors
(b) driving moment of the motors
Figure 8. Simulation results ( 18 m/min
p
v = ,
2
20 m/s
p
a =
)
Let 0.0045 m =
sim
r , 66.7 rad/s =
sim
, the
polishing feedrate is 18m/min, the acceleration could
reach
2 2
20m/s =
sim sim
r , which is twice the gravity
acceleration. The simulation results are shown in Fig. 8.
From Fig.8 (b) we can see that the driving moments are
less than 6.5 Nm. The result also demonstrates that the
PKPM can bear much higher acceleration, so that the
presented PKPM has a high dynamic performance.
4. Tracking error control based on the inverse dynamics
4.1 Methods for tracking error control
Currently, there exist many control techniques, such as
PID control, computed torque control, slide control, fuzzy
control, robust control and so on. The parallel
manipulator has a complex structure. From Eq.(62) we
know that the dynamic parameters of the parallel
mechanism are timevarying.
In the polishing process the trajectory of the polishing
tool requires it to be smooth, as the sharp motion and
vibration of the polishing tool will deeply affect the
polishing precision. As is well known, the slide mode
control keeps the robotic motion state around the sliding
mode surface, but the switch function of the sliding mode
control could make the robot, at a highfrequency, chatter
around the sliding mode surface; the chattering would
result in a low control accuracy and joint wear occurring
quickly.
The friction component
( )
F q
and q is used to
denote q
, , ,
M q M q M q
C q q C q q C q q
G q G q G q
= +
= +
= +
(64)
where
( )
M q
,
( )
, C q q
and
( )
G q
are the nominal
parametric matrices, and
( ) M q
,
( ) , C q q
and
( ) G q
are the uncertainties. Let
d
denote a finite
energy exogenous disturbance force,
d
is divided into a
constant disturbance component
c
d
and a variable
disturbance
v
d
, i.e.
c v
d d d
= + , they both are
unknown in the inverse dynamic equation, so Eq. (63)
with a disturbance force can be rewritten as,
( ) ( ) ( )
,
c v
d d
M q q C q q q G q + + = + +
(65)
with
( )
( ) ( ) ( ) ( )
,
c
d
v
d
G q
M q q C q q q F q
= + +
(66)
The tracking error of the motion controller decides the
motion precision of a robot or machine. For a given
motion control object, the best way is to build up its
dynamic model with the exact parameters and all the
disturbance information, but in the real world there will
be some information that cannot be exactly measured or
obtained so we need to attenuate the effects of the
unknown parameters and the perturbation on the
tracking error. For a given trajectory of the moving
platform,
d
q
, with its first and second order derivatives,
d
q
and
d
q
, the tracking error vector is defined as,
d
d
q q q
x
q q q
( (
= =
( (
(67)
The minimized control action, which includes the
minimal applied torque and energy consumption on the
motion control, is introduced by R. Johansson [27],
considering a more general control action, a state
transformation is given by
1
0
11 12 2
0 z I
z T x x
T T z
( (
= = =
( (
(68)
The applied torque with a variable u for the assignment
of the control law is introduced into the robust control
design by a minimized control action [26, 27].
( ) ( ) ( )
( ) ( )
( ) ( ) ( ) ( )
1 1 1
12 11 12 0
,
, , ,
T
d
d
M q q T T q T M q C q q B Tx u
C q q q F q G q q q q
= +
+ + +
(69)
Substituting Eq. (69) and Eq. (67) into Eq. (65), the tracking
error model based on unknown parameter matrices and
external disturbances can be obtained,
( ) ( ) ( ) , , ,
T T T
x A x t x B x t u B x t w = + +
(70)
where
( )
( ) ( )
( ) ( )
( ) ( )
1 1
12 11 12
1
0 0
1
1 1
0
1
12
,
0 ,
0
,
T
T
d
T T T
A x t T T
M q C q q
B x t T M q
I
w M q T M q
(
= (
(
(
=
(
=
(71)
Let disturbance
d
tracking
error performance for the positive definite matrix
1 f
Q
can be expressed as,
( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( )
1 1
2
1 1
0
,
d
f
T
f
t
T T T
J u w x t Q x t
x t Q x t u t Ru t w t w t t
= +
+ +
}
(72)
The
2
and
performance indices.
The solution of the minimax problem described by Eq. (74)
and Eq. (75) turns out to solve a coupled nonlinear
timevarying Riccatilike equation, but the nonlinear
Riccatilike equations are too difficult to use in
engineering. The simple solution for the
2
performance tracking error problem is given by
introducing some restrictions [26]. For a desired
disturbance attenuation level , with
0 1 < <
and
the positive definite weighting matrix of
, the
performance index can be written as,
T
11 11 12
1
21 22 22
T
Q Q Q
Q
Q Q Q
(
=
(
(76)
where
22 22
Q q I =
and
T T T
11 22 22 11 12 12
Q Q Q Q Q Q + > + ,
then the weighting matrix R and the state translation
matrix T can be obtained,
2
1
2
R I
 
=

\ .
(77)
0 1/ 2 1/ 2
11 22
0
(1 ) (1 )
I
T
Q Q
(
=
(
(78)
Hence, the mixed
2
= +
+ + +
(79)
with
1 T
0
u R B T x
=
(80)
Eq. (79) and Eq. (80) can be rewritten in the form of a
computed torque,
( )( )
( ) ( ) ( )
,
d v p
d
M q q K q K q
C q q q F q G q
= +
+ + +
(81)
with mixed
2
,
v
p
K T T M q T R T
K M q C q q T T M q T R T
= +
= +
(82)
Compared to the classical static gain computed torque
control, Eq. (82) shows that the mixed
2
performance control has dynamic gain. Hence, the mixed
16 Int. j. adv. robot. syst., 2013, Vol. 10, 314:2013 www.intechopen.com
2
control is
degenerated into the
2
optimal control.
4.3 Simulations with four control methods
In this section the computed torque method, the optimal
control method, the
method.
The tracking error control focuses on control precision, the
attenuation of disturbance, stability and computed
driving torque. Four control methods are discussed in this
paper. The following simulation is concerned with the
disturbance condition. The disturbance is divided into
two components, the constant component
c
d
and the
variable component
v
d
. Fig. 9 shows the disturbance
inputs in the simulation.
Case 1: Continuous trajectory command input with
disturbance
c
d
and
v
d
The continuous trajectory command input is shown in Fig.
10. Each trajectory component of the generalized variables
of the moving platform is a sinusoidal function relative to
the home configuration [ ]
T
0, 0, 0.8, 0, 0
.
The simulation results of the continuous trajectory
command are shown in Fig. 11(a)(d). We can see that the
optimal control obtains the highest tracking error
precision and the lowest one is
control. This is
because the optimal controller is only designed to obtain
the high tracking performance and the
controller is
only designed to attenuate the effect of disturbance and
unknown parameters. The mixed
2
/
control has a
higher tracking error precision than those obtained from
computed torque control and
control.
Case 2: Step trajectory command with disturbances
c
d
,
v
d
c
d
50Kg
O
v
d
50Kg
t
50Kg
(a) Constant disturbance (b) Variable disturbance
Figure 9. Disturbances for simulation
Figure 10. Continuous trajectory command relative to the home position
0 1 2 3 4 5
0.2
0.1
0
0.1
0.2
Time (s)
P
o
s
i
t
i
o
n
(
m
)
Continuous Trajectory
x
y
z
17 Weiyang Lin, Bing Li, Xiaojun Yang and Dan Zhang: Modelling and Control
of Inverse Dynamics for a 5DOF Parallel Kinematic Polishing Machine
www.intechopen.com
(a) Computed torque method
(b) Optimal control
(c)
control
(d) Mixed
2
/
control
Figure 11. Simulation results of continuous trajectory command with disturbances
[ ]
[ ]
T
T
0, 0, 0.8, 0, 0 0
0, 0, 0.7, 0, 0 0
d
t
q
t
<
(83)
The position [ ]
T
0, 0, 0.8, 0, 0 is the origin of the PKPM,
the step command trajectory moves the robot to
[ ]
T
0, 0, 0.7, 0, 0 at initial time
0
t
. The step command is
shown in Fig. 12.
0 1 2 3 4 5
3
2
1
0
1
2
3
Time (s)
D
r
i
v
i
n
g
T
o
r
q
u
e
(
N
m
)
Computed Torque
5
0 1 2 3 4 5
6
4
2
0
2
4
x 10
3
Time (s)
E
r
r
o
r
(
m
)
Tracking Error
e
x
e
y
e
z
e
0 1 2 3 4 5
4
2
0
2
4
Time (s)
D
r
i
v
i
n
g
T
o
r
q
u
e
(
N
m
)
Optimal Control
5
0 1 2 3 4 5
3
2
1
0
1
2
3
x 10
5
Time (s)
E
r
r
o
r
(
m
)
Tracking Error
e
x
e
y
e
z
e
0 1 2 3 4 5
3
2
1
0
1
2
3
Time (s)
D
r
i
v
i
n
g
T
o
r
q
u
e
(
N
m
)
H
Control
5
0 1 2 3 4 5
6
4
2
0
2
4
6
x 10
3
Time (s)
E
r
r
o
r
(
m
)
Tracking Error
e
x
e
y
e
z
e
0 1 2 3 4 5
3
2
1
0
1
2
3
Time (s)
D
r
i
v
i
n
g
T
o
r
q
u
e
(
N
m
)
Mixed H
2
/H
Control
5
0 1 2 3 4 5
6
4
2
0
2
4
x 10
4
Time (s)
E
r
r
o
r
(
m
)
Tracking Error
e
x
e
y
e
z
e
and
variable disturbance
v
d
.
(a) Computed torque method
(b) Optimal control
(c)
control
(d) Mixed
2
/
control
Figure 13. Simulation results of step trajectory command with disturbances and torque limit
z
t O
0.1
0 1 2 3 4 5
15
10
5
0
5
10
15
Time (s)
D
r
i
v
i
n
g
T
o
r
q
u
e
(
N
m
)
Computed Torque
5
0 1 2 3 4 5
0.02
0
0.02
0.04
0.06
0.08
0.1
Time (s)
E
r
r
o
r
(
m
)
Tracking Error
e
x
e
y
e
z
e
0 1 2 3 4 5
15
10
5
0
5
10
15
Time (s)
D
r
i
v
i
n
g
T
o
r
q
u
e
(
N
m
)
Optimal Control
5
0 1 2 3 4 5
0.2
0.1
0
0.1
0.2
0.3
Time (s)
E
r
r
o
r
(
m
)
Tracking Error
e
x
e
y
e
z
e
0 1 2 3 4 5
1
0.5
0
0.5
1
Time (s)
D
r
i
v
i
n
g
T
o
r
q
u
e
(
N
m
)
H
Control
5
0 1 2 3 4 5
0.02
0
0.02
0.04
0.06
0.08
0.1
Time (s)
E
r
r
o
r
(
m
)
Tracking Error
e
x
e
y
e
z
e
0 1 2 3 4 5
15
10
5
0
5
10
15
Time (s)
D
r
i
v
i
n
g
T
o
r
q
u
e
(
N
m
)
Mixed H
2
/H
Control
5
0 1 2 3 4 5
0.15
0.1
0.05
0
0.05
0.1
Time (s)
E
r
r
o
r
(
m
)
Tracking Error
e
x
e
y
e
z
e
19 Weiyang Lin, Bing Li, Xiaojun Yang and Dan Zhang: Modelling and Control
of Inverse Dynamics for a 5DOF Parallel Kinematic Polishing Machine
www.intechopen.com
The computed torque controller can track the step
trajectory command well, but when the disturbance
c
d
and
v
d
control method is too low to use in tracking the step
trajectory command.
From the figure we can see that the tracking error is still
smooth when the disturbances
c
d
and
v
d
appear at
time t from 1.25s and 2.5s. This implies that the mixed
2
control
method has a high combination performance of tracking
error control and attenuation of external disturbances.
Considering the worst conditions of the step trajectory
command with disturbances and driving torque limit, the
mixed
2
control method is capable of the attenuation of
disturbance with a high tracking error performance.
5. Conclusions
A 5DOF PKPM with a motion type of T3R2 and a
redundant linear motion actuator is developed in this
paper, there are two advantages to the structural scheme:
one is the large yaw angle workspace which is suitable for
polishing parts with freeform surfaces; the second is that
the PKPM could perform constant polishingforce control
with the 3D force sensor and linear motion actuator
mounted on the moving platform. This paper focuses on
the inverse dynamic modelling and tracking error control.
The kinematic analysis was developed and a closure
vector method was introduced to express the geometric
relations of the PKPM linkages, which could avoid a lot of
tedious derivations in formulating the linear velocities
and angular velocities. The affine projection method was
presented as a means to deal with the angular velocities
and the angular accelerations projection problem
efficiently, so that the linear velocities and the linear
accelerations at the mass centre are obtained. Based on
these variables an inverse dynamic algorithm based on
Kanes equation is developed: it is highly efficient and
works in realtime to obtain the driving control torque of
the motors. The inverse dynamic driving torque
simulation results show that the parallel kinematic
machine can work at high speeds and high acceleration.
The presented PKPM can achieve an acceleration two
times that of gravity, while the conventional machine tool
can only achieve an acceleration of one time greater than
gravity.
The mixed
2
Bien plus que des documents.
Découvrez tout ce que Scribd a à offrir, dont les livres et les livres audio des principaux éditeurs.
Annulez à tout moment.