ADAPTIVE CONTROL OF MECHANICAL SYSTEMS
WITH STATIC AND DYNAMIC FRICTION
COMPENSATION
By
YONGLIANG ZHU
Bachelor of Science
Zhejiang University
Hanzhou, Zhejiang, P.R. China
1988
Master of Science
University of Science and Technology
Hefei, Anhui, P.R. China
1991
Submitted to the Faculty of the
Graduate College of the
Oklahoma State University
in partial fulfillment of
the requirements for
the Degree of
MASTER OF SCIENCE
December, 2001
ADAPTIVE CONTROL OF MECHANICAL SYSTEMS
WITH STATIC AND DYNAMIC FRICTION
COMPENSATION
Thesis Approved:
Thesis Adviser
_2~~ A.~'
Dean of~ate College
ii
ACKNOWLEDGMENTS
I wish to express my sincerest appreciation to my advisor, Dr. Prabhakar R. Pagilla
for intelligent supervision, constructive guidance, inspiration, and friendship throughout
the development of this investigation. Without his help and supervision, I would not have
completed this work.
I would like to extend my warmest thanks to my master committee members: Dr. Gary
Young and Dr. E.A. Misawa for their help and guidance.
I wish to thank my colleagues at Oklahoma State University Robotics Research Lab:
Rarnmishly V. Dividedly, Fu Yet, Gymkhana Nag, Sachem, and L. Prase C. Parera. They
are among the finest people I know and are a joy to work with.
III
TABLE OF CONTENTS
Chapter Page
1 Introduction 1
2 Dynamic Model with Friction 4
2.1 Robot Dynamics ... 4
2.2 Static Friction Models. 6
2.3 The Dahl Model. . . . 8
2.4 The BlimanSorine Model 9
2.5 The LuGTe Model . . . . . 11
2.6 Complete Dynamic Model 12
3 Controller Design 14
3.1 PD Control 15
3.1.1 Simulation Results 15
3.2 Model Based Adaptive Control 17
3.3 Computed Torque Control .. 20
3.4 Model Based Adaptive Control with Static Friction Compensation 21
3.4.1 Simulation Results . . . . . . . . . . . . . . . . 24
3.5 Adaptive Control with Dynamic Friction Compensation . 26
3.5. 1 Simulation Results . . . . . . . . . . . . . . . . 29
4 Experimental Platform 32
4.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 33
iv
Chapter Page
4.1.1 Motor Drivers. . . . . 34
4.1.2 DSP Servo Controller 35
4.2 Software........ 36
4.2.1 RPL Software. 36
4.2.2 Servo Software 37
4.3 Manipulator Kinematics 37
4.4 Manipulator Dynamics 39
5 Experiments 43
5.1 Desired Trajectory Generation 43
5.2 Experimental Condition. 45
5.3 Motor Characteristics. . 46
5.4 Parameter Identification of the LuGre Friction Model 48
5.5 Experimental Results 51
5.5.1 PD Control . 51
5.5.2 Model Based Adaptive Control. 54
5.5.3 Computed Torque Control . . . 55
5.5.4 Model Based Adaptive Control with Static Friction Compensation 56
5.5.5 Adaptive Control with Dynamic Friction Compensation 59
5.6 Comparison of Experimental Results. . . . . . . . . . . . . . . 59
6 Summary and Future Work 71
6.1 Summary . . 71
6.2 Future Work. 74
BffiLIOGRAPHY 76
v
Chapter
A Manipulator Application Software
A.I Servo.c
A.2 Rpl.c ..
B MATLAB Programs
B.l Pddiff.m .....
B.2 Pdcontrolload.m .
B.3 Adptvfpayloadl.m
B.4 Adptvfl.m.
B.5 Lugre.m ..
8.6 Lugrediff.m .......
VI
Page
79
79
. . 100
119
· 119
... 121
... 123
...... 124
· 127
· 131
LIST OF FIGURES
Figure Page
2.1 Static friction models: (a) Coulomb, (b) Coulomb + Viscous, (c) Coulomb
+ Viscous + Stiction, (d) Stribeck . 6
2.2 The Dahl friction model. . . . 8
2.3 BIimanSorine friction model. 10
3.1 PD control loop of the manipulator . 15
3.2 Trajectory in Cartesian coordinate 16
3.3 Angular position and angular position error 16
3.4 Tracking performance in the presence of payload 17
3.5 Adaptive control + static friction compensation simulation result: position
and tracking error . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.6 Adaptive control + static friction compensation simulation result: estimated
parameters (fi}, fh, P3) . . . . . . . . . . . . . . . . . . . . . 25
3.7 Adaptive control + static friction compensation simulation result: esti
3.8 Adaptive control + static friction compensation simulation result: tracking
performance in the presence of payload .. . . .
3.9 Simulation: position, velocity and tracking error.
3.10 Simulation: estimated parameters.
4.1 Sketch of the robot system
4.2 Hardware architecture. . .
vii
25
26
30
31
32
33
Figure Page
4.3 Torque mode vs. velocity mode. . . . . 35
4.4 Connection diagram of the robot system 36
4.5 Diagram of the work space of the manipulator . 37
4.6 Diagram of the twolink planar NSK manipulator 39
5.1 Circle trajectory .................. 43
5.2 Diagram of (id, C/d, qd and T for a circle trajectory . 44
5.3 Free body diagram of DC motor .. 47
5.4 Identification of f32 and Fc for link 1 52
5.5 Identification of f32 and Fc for link 2 52
5.6 Identification of Ws for link ] 53
5.7 Identification of Ws for link 2 53
5.8 Identification of f30 for link 1 54
5.9 Identification of f30 for link 2 55
5.10 PO control experimental result (circle trajectory) 56
5.11 Adaptive control experimental result: velocity and tracking error (circle
trajectory) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 57
5.12 Adaptive control experimental result: estimated parameters $1, P2, P3) (circle
trajectory) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 57
5.13 Computed torque control experimental result: velocity and tracking error
(circle trajectory) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 58
5.14 Adaptive control + static friction compensation experimental result: velocity
and tracking error (circle trajectory). . . . . . . . . . . . . . . . . . .. 58
5.15 Adaptive control + static friction compensation experimental result: estimated
parameters (Fv(l, 1), Fv(2, 2), Fc(1, 1), Fc(2, 2» (circle trajectory) . 59
Vll1
Figure Page
5.16 Adaptive control with dynamic friction compensation experimental result:
velocity and tracking error (circle trajectory) . . . . . . . . . . . . . . . .. 60
5. 17 Adaptive control with dynamic friction compensation experimental result:
estimated parameters CPI, P2, P3) (circle trajectory) 60
5.18 Adaptive control with dynamic friction compensation experimental result:
estimated parameters (Qo(l, 1), Ql(l, 1), Q3(l, 1)) (circle trajectory). . .. 61
5.19 Adaptive control with dynamic friction compensation experimental result:
estimated parameters (Qo(2, 2), QI (2,2), Q3(2, 2)) (circle trajectory). . .. 61
5.20 Adaptive control with dynamic friction compensation experimental result:
estimated parameters (Z01, Zll. Z02, Z12) (circle trajectory) .
5.21 Comparison of tracking errors for link 1 (circle trajectory)
5.22 Comparison of tracking errors for link 2 (circle trajectory)
5.23 Tracking error and its 2norm for link 1 (circle trajectory) .
5.24 Tracking error and its 2norm for link 2 (circle trajectory) .
5.25 Desired trajectories for link 1 and 2 (Sine trajectory) ..
5.26 Comparison of tracking errors for link 1 (Sine trajectory)
5.27 Comparison of tracking errors for link 2 (Sine trajectory)
5.28 Estimated static friction parameters (Sine trajectory) ..
~ ~ ~
5.29 Estimated dynamic friction parameters (Qo, Q1, Q3) for link 1 (Sine trajec
62
64
64
65
65
66
66
67
67
tory) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 68
5.30 Estimated dynamic friction parameters (Qo, QI, Q3) for link 2 (Sine trajectory)
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 68
5.31 Adaptive control with dynamic friction compensation experimental result:
estimated parameters (z()l, Z11> Z02, Z12) (sine trajectory) .
5.32 Tracking error and its 2norm for link 1(Sine trajectory) .
5.33 Tracking error and its 2norm for link 2 (Sine trajectory)
IX
69
69
70
e = q  qd
e= q qd
f
q
q
q
M(q)
 M(q)
C(q, q)
C(q, q)
g(q)
g(q)
NOMENCLATURE
Position tracking error in joint space
Velocity tracking error in joint space
Extemal force
Real position in joint space
Real velocity in joint space
Real acceleration in joint space
Desired position in joint space
Desired velocity in joint space
Desired acceleration in joint space
Reference position in joint space
Reference velocity in joint space
Reference acceleration in joint space
Reference velocity error
Mass matrix of robot
Estimate of M
Matrix composed of Coriolis and centripetal terms of robot
Estimate of C
Gravity vector
Estimate of 9
x
J (q) Jacobian matrix
T Control Torque
AI;) Derivative feedback gain
Ap Proportional feedback gain
{3 Manipulator parameters (Pl' P2, Pa)T
/3 Estimates of /3
/3 Estimates error = /3  /3
Qi Friction gain
 Qi Estimate of Qi
Fc Coulomb friction parameter
Fv Viscous friction parameter
V s Stribeck velocity
" a Parameters in Dahl friciton model
x Displacement
v Velocity
Z, Zij Internal state of LuGre friction model
Z, Zij Estimate of z. Zij
Z, Zij Estimate error i = Z  Z, Zij = Zij  Zij
/3i Friction parameter
f f Friction force/torque
xi
CRAPfERl
Introduction
Robotic applications sometimes involve unknown and hazardous environment, which
require demanding control performance that cannot be achieved by conventional robot controllers.
The requirements of accurate and stable endeffector trajectory tracking of highspeed
manipulators under dynamic and payload uncertainties, sensor noise and external
disturbances, have initiated the research in adaptive robot controllers; a vast amount of
literature exists in the area ([1], [7] [3] [4]). Within this context, two major classes of adaptive
control schemes can be identified, namely, the PerfonnanceBased Adaptive Control
(nonregressor methods) and the ModelBased Adaptive Control (regressor methods). In
the performancebased approach, the controller gains are adjusted based on a system performance
index. These controllers ordinarily need very little information about dynamic
parameters and system structure. They are simple, requiring little realtime computations
and have shown good tracking performance in laboratory experiments.
The modelbased approach consists of a nonlinear compensation of the robot model,
whose dynamic parameters are estimated through an adaptation law, generally derived
from Lyapunov Theory, which guarantees asymptotic stability. Adaptive control techniques
are robust to parameter uncertainty, but not robust to unmodeled dynamics and
disturbance. More recently, Fuzzy control [19J, Neural Network control [20]. Learning
control ([21],[22]) techniques have been proposed to handle the trajectory tracking problem
of the robot.
The turning point in the solution of tracking problems in the adaptive control law was
proposed by Slotine and Li ([2], (3)), which consisted of a linear part PDtype and a non
1
linear modelbased part which utilizes feedforward and feedback actions based on position
and velocity data. The control law requires position and velocity measurement, but avoids
inverse inertia matrix computation and acceleration measurement which is usually not possible
in many robots. The applied torque is usually filtered to avoid high frequency noise
before applying to the actuators of the robot
High accuracy tracking cannot be achieved if friction effect is not considered properly.
Friction is a complicated phenomenon, which exhibits coulomb friction, viscous friction
and nonlinearity at low velocity. To cancel the friction effect, friction compensation is
usually used ([10)[16]). The classic friction models do not sufficiently describe all the
dynamic effects of friction, such as presliding, the friction lag and Stribeck effect. These
phenomena all occur in the low velocity and presliding region. Many researchers are still
working on friction modelling, yet there is no general agreement on which one is the best.
To incorporate more dynamic friction characteristics when designing a friction compensation,
three empirical phenomenological models are usually applied to the physical process
([10), [11), [12]). They are the Dahl model, the HlimanSorine model. and the LuGre
model. The BlimanSorine model and the LuGre model are extensions of the Dahl model.
The LuGre model gives richer behavioral description of friction phenomena than the Dahl
model and the BlimanSorine model. The BljmanSorine model may be problematic to use
because of its poor damping properties at zero crossings of velocity[10).
The previous work dealing with the friction compensation has the following limitations:
(1) only static friction was considered, i.e, only static friction behaviors such as
viscous friction, Coulomb friction and Stribeck effect are considered, (2) simple mechanical
structure like one link structure was considered when dynamic friction compensation
was applied, and the dynamic model of the mechanical system studied is assumed without
unknown parameters, and further (3) very little has been done to experimentally investigate
the proposed designs and methodologies for multilink robot applications.
This thesis has not only considered theoretical developments but also investigated the
2
validity of the theoretical developments via extensive experimentation. In the adaptive
control with dynamic friction compensation, both the robot model parameter uncertainty
and the dynamic friction parameter uncertainty are considered for multilink robots. Dynamic
parameter uncertainties and friction are the principal sources of the tracking error.
Robots usually face uncertainties on parameters describing the dynamic properties of the
grasped load, such as moments of inertia or exact position of the center of the mass in the
endeffector. Simulation and experiments based on the proposed adaptive controller with
friction compensation techniques are conducted on a twolink planar manipulator.
This work has the following contributions: (1) adaptive position control with static
friction compensation which considers Coulomb and viscous effects was proposed; (2)
Adaptive position control with dynamic friction compensation, where the LuGre friction
model was applied, was proposed; (3) extensive experiments were conducted to investigate
different controllers for robotic tracking task, and their results were compared; (4) different
friction models commonly used were investigated; (5) NSK manipulator system software
was studied, and some modifications were made. This will facilitate the future work in this
manipulator.
The thesis is organized as follows. Chapter 2 describes the robot dynamics and friction
dynamics. Chapter 3 introduces five controllers used for investigation in this thesis. The
control law for each controller is given and stability is proved. Chapter 4 discusses the control
system architecture of the twolink manipulator, on which the proposed controllers are
implemented. Chapter 5 provides the experimental results. Summary and future research
are given in Chapter 6. Appendix gives the source code written for the experiment and
simulation.
3
CHAPTER 2
Dynamic Model with Friction
Dynamic modelling plays an important role in control design. In this chapter, the dynamics
of a robot with friction is developed. Section 2.1 discusses the robot dynamics
derived based on EulerLagrange equations, and some properties of the robot dynamic
model, which are essential for control. High performance of position tracking control of
robot system cannot be achieved if friction phenomena are not properly taken into account.
Failing to compensate for friction in robot applications can lead to large tracking errors
and possibly limit cycles when velocity reversals (velocity crossing zero point) are demanded.
Understanding of friction is very important for friction compensation design in
control systems. However, friction is a very complicated phenomena and its parameters
vary with temperature and age. Various static friction models are discussed in Section 2.2.
The dynamic friction models are being actively studied, and there is no general agreement
on which model is the best to be employed. To incorporate more dynamic friction characteristics,
three empirical phenomenological models are usually applied to the physical
process ([10], [11], [12]). They are the Dahl model, the BlimanSorine model, and the LuGTe
model. Section 2.3, 2.4 and 2.5 give detailed discussion of these three friction model.
The complete dynamic model with friction is presented in Section 2.6.
2.1 Robot Dynamics
Let the kinetic and potential energy function of an nlink robot be given by K(q, q) =
~qTM(q)q and P(q), where q E jRn, q E jRn are the generalized position and velocity, respectively,
and M(q) E IRnxn is the symmetric positive definite mass matrix. The dynamics
4
of the robot is given by
M(q)ij + C(q, q)q + g(q) = T + JT (q)f + f/(q, q) (2.1)
where C(q) q) is the matrix composed of Coriolis and centripetal tenns, g(q) is the gravity
vector, T is the vector of generalized forces applied by the motors at each joint of the robot.
!J(q,q) is the vector of friction torque. f represents the vector of external forces and J(q)
is the Jacobian of the manipulator.
The dynamic model of the robot is assumed to satisfy the following well known properties:
• The inertia matrix M(q) is a symmetric positive definite matrix, and is bounded, i.e.,
(2.2)
where Amin and Amax denote the strictly positive finite minimum and maximum
eigenvalue of M(q) for all q, respectively.
• The matrix of ~M(q)  C(q q) is skewsymmetric, that is,
T 1 .
z [2 M(q)  C(q, q)]z = 0
for any vector z E JRn
(2.3)
• The lefthandside of the dynamics given by (2.1) is linear in tenns of coupled manipulator
inertial parameters, and can be written as
M(q)ij + C(q, q)q g(q) = Y(q, q, ij){3 (2.4)
where {3 E lRP is a coupled manipulator parameter vector, and Y(q, q, q) is a n x p
regressor matrix.
The first two properties are essential in Lyapunov stability analysis. The adaptive control
algorithms are based on the linear parameterization property of the dynamics of robot
manipulator.
5
v v
(c)
ff ff
Kinematic Fc Fc Kinematic
+ Viscous Friction
v v
(a) (b)
ff ff
Static Fs
Figure 2.1: Static friction models: (a) Coulomb, (b) Coulomb + Viscous, (c) Coulomb +
Viscous + Stiction, (d) Stribeck
2.2 Static Friction Models
Figure 2.1 shows the diagram of the four static friction models. In Figure 2.1, (a), (b),
(c) are the classic fri.ction models which cover the Coulomb, Coulomb + Viscous, Coulomb
+ Viscous + Stiction effects, respectively, and the Stribeck effect is included in Cd). In
dynamic systems, the Coulomb friction is often modelled as a piecewise function which
is a positive function for positive velocity, a negative function for negative velocity, and
zero for zero velocity. Although Coulomb friction provides constant friction for nonzero
velocity, it introduces discontinuity at zero velocity. It is like a "relay nonlinearity". This
nonlinearity may produce limit cycles in closedloop control systems. As a result, poor
control accuracy is achieved.
Viscous friction is represented as a linear function of velocity. It results from the viscous
behavior of a fluid lubricant layer between two rubbing surfaces. Static friction is the force
required to initiate motion from rest. The magnitude of the static friction is greater than
6
that of kinematic friction. Classic friction compensation usually handles only the Coulomb
effect because the model is simple and this kind of frict"on effect is dominant in mechanical
systems if the velocity does not frequently cross the zero point.
The friction models shown in Figure 2.2 (a), (b), (c), (d) are mathematically represented
in the following equations.
(a) Coulomb friction
(2.5)
(b) Coulomb + Viscous friction
(c) Coulomb + Viscous + Stiction friction
(2.6)
{
Fesgn(v) + Fcv h=
Fasgn(F)
(d) Coulomb + Viscous + Stribeck
for v =I a
for v = a
(2.7)
where
p. Coulomb friction coefficient c'
Fa: Stiction friction coefficient
Fv: Viscous friction coefficient
Va: Stribeck velocity constant
F: Applied tangential force
v: Velocity
sgn(): Sign function
7
(2.8)
Fe
~ <:: u..
i:i c
.H ~
II "C > u..
Timet TImet
Displacement x
Fe ,,:;::11
Velocfty v
Figure 2.2: The Dahl friction model
2.3 The Dahl Model
In the late 1960's, Dahl provided a dynamic friction model (known later as the Dahl
model) given by
where
cr: Rest stiffness parameter
a: Solid friction exponent parameter
x: Relative displacement
In time domain, Equation (2.9) can be written as
(2.9)
(2.10)
The exponent a is empirically given by Dahl as ()' ~ l}).
The Dahl model is inspired by the stressstrain characteristics from solid mechanics.
Figure 2.2 shows Dahl's friction force response for a sinusoid velocity. It can be seen that
the Dahl model behaves like a spring for small deflection x, and reaches Coulomb friction
8
model for large deflection. Friction force will monotonically increase/decrease to ±FlI •
Coulomb friction model neglects the small deflection zone behavior; and the friction force
jumps to ± Fs directly.
The steady state version of the Dahl model for (/=1 is
&
If = Fcsgn(v) (2.11)
(2.12)
which is the Coulomb friction model. The Dahl model accounts for Coulomb friction but
it does not include the Stribeck effect.
2.4 The BlimanSorine Model
Bliman and Sorine developed a dynamic model which emphasizes the importance of
rate independence. i.e., the friction force is not explicitly dependent on the velocity, but
only on the distance travelled after a velocity zero crossing. The model is expressed as a
linear system in the space variable s, which is given by
s = it IV(T)ldT
~s = Axs + Bsgn(v)
If = CXs
where A, B, C are matrices, and v is the velocity.
For the second order model, it is suggested to choose the matrices as follows:
(2.13)
In time domain, the time derivative of friction force is
(2.14)
(2.15)
9
0.5
Time.
I.eady ut.." 12
1 L..__~ _____'
o
_ 0.51\
~ ,
~ 0
lL
0.5
0.5
Timet
3 0
~
0.5
0.5
0.1 0.2 03 0.4
Displacement x(l)
o
Voloellyv(t)
,
;' Itletlon peak
.~ h
~
1,
0.5
0.5
1 L.._~~~~_ _____'
o
_ 05 \.
~ ......, lL
~ 0 g
lL
Figure 2.3: BlimanSorine friction model
Hence, the second order BlimanSorine model can be viewed as a parallel connection of a
fast and a slow Dahl model.
Second order BlimanSorine model has the following properties:
• The difference II  h determines the amplitude of friction at steady state. This can be
observed from Equation (2.15). At steady state, Xsl = IIsgn(v), X s2 =  hsgn(v),
hence 11 = Xsl + Xsl = (fl  h)sgn(v) .
• BlimanSonne model incorporates the stiction effect; this can be deduced by linearizing
Equation (2.14) at zero states and zero velocity:
oII = fol vOt  hovot = (IcI5X h) () s l  OXs 2 sgn ov
7]ef ef 7]e1 f.1
(2.16)
Figure 2.3 shows the characteristics of the BlimanSorine model due to a sinusoid velocity
input, where the stiction effect and Dahl's effect at low velocity (at small motion around
zero crossings of velocity, friction behaves like a spring) can be observed.
lO
2.5 The LuGre Model
The LuGre model was presented in [11]. The standard fonnulation describing the dynamic
friction is as follows:
4
dz = Ivl v  13oz
dt g(v)
g(v) = Fc + (Fa  Fc )e(v/v.)2
ff = 130z +131Z + f(v)
where
z: The average deflection of the bristles which is unmeasurable
130: Stiffness coefficient
131: Velocity dependent damping coefficient
f (v ): Function including effects such as viscous friction and lubrication
Va: Stribeck velocity
Fa: Stiction friction force constant
(2.17)
Fc : Coulomb friction force constant
The function g(v) is positive and depends on many factors such as material properties,
lubrication, temperature; g(v) will decrease monotonically from g(O) when v increases;
g(v) can be chosen to describe different friction effects; g(v) chosen in Equation (2.17)
characterizes the Stribeck effect. The function f (v) is usually chosen as f (v) = 132V; it
represents viscous friction.
The LuGre model considers the dynamic effects of friction arising out of the deflection
of bristles which model the asperities between two contacting surfaces. The contacting
surfaces can be visualized as consisting of many asperities at the microscopic level. When
a slight tangential force is applied, the bristles win deflect like elastic springs. If the force
is large enough, the bristles start to slip.
If g(v) = Fe l130 and 131 = fh = 0, Equation (2.17) becomes
dff = I30v(1f Jsgn(v))
dt Fc
11
(2.18)
the LuGre model reduces to the Dahl model. The steady state of the LuGre model is:
&
Z= g(v)sgn(v)
130
If = g(v)sgn(v) + I(v)
(2.19)
The LuGre model captures many aspects offriction, including stiction, stick slip, stribeck,
hysteresis and zero slip displacement. The LuGre model is characterized by six parameters:
(30, (31) (32, vs' Fe and Fs. The identification procedure for these parameters is described in
[13]. This method is based on a series of experiments in different friction regimes. The
constants (30 and (31 are identified in the stickslip region, and vs' Fe and Fs are identified
in steady state region. The function I (v) is identified from break away experiments.
2.6 Complete Dynamic Model
Substituting the static and dynamic models, described in Equation (2.6) and Equation
(2.17), into the robot dynamics, Equation (2.1), yields the following complete dynamic
model. 3.
• Complete dynamics with static friction model is
M(q)q +C(q, q)q +g(q) + Fesgn(q) + Feq = T
Jf = Fesgn(q) + Fvq
(2.20)
(2.21)
where Fv = diag(Fv1 , Fv2 , .", Fvn ) and Fe = diag(Fcl , Fe2 , ... , Fen) are diagonal
matrices, FVi and Fa are the Coulomb and viscous friction coefficients of the i th
link, respectively.
• Complete dynamics with dynamic friction model is
dz .
 = q  \liz
dt
if = Qoz + Q1 Z+ Q2q
12
(2.22)
(2.23)
where Qo, Q1 and Q2 are diagonal matrices whose diagonal elements correspond to
the friction constants as given by (2.17), and Wis a diagonal matrix and is given by
W = diagh'11q111g1 (qd,· .. ,1'nlqnllgn(qn))
9i(qi) = Fci + (Fsi  Fci )e(4i/W ,i?
where diag(a], ... ,an) represents the diagonal matrix with diagonal elements a1
through an and Wsi is the Stribeck velocity corresponding to joint i. Substitution of
the dynamic friction model (2.22) and (2.23) into the robot dynamics and simplifying
we obtain
M(q)ij + C(q, q)q + g(q) + Q3q + Qoz  Q1 \{1z = T (2.24)
13
CHAPTER 3
Controller Design
This chapter will investigate various control designs for the complete robot model given
in Chapter 2. Five types of control designs are considered: (1) PD controller (PD), (2)
computed torque control (CT), (3) adaptive control (AC), (4) adaptive control with static
friction compensation (ACS) and (5) adaptive control with dynamic friction compensation
(ACD). Modelbased computed torque control and adaptive control are wen known in
literature[18] and a brief explanation of each is given. The adaptive control design is augmented
with static friction compensation and dynamic friction compensation and stability
of the closedloop system is shown in each case. The goal of each designed controller is to
get good tracking performance, i.e.• achieve the smallest tracking error that is possible.
PO controller is the most commonly used controller in industrial environment because
of no dynamic model requirement and easy implementation. Simulation and experimental
results of PD controller are shown; these results are used to compare with that of other
control design.
In computed torque control, the robot parameters are assumed to be known. The other
three controller (AC, ACS, ACD) do not require accurate knowledge of robot parameters.
Furthermore, ACS and ACD consider static friction compensation and dynamic friction
compensation, respectively.
Some simulation results on a twolink manipulator (refer to Section 4) are shown in this
Chapter. The experimental results are presented in Chapter 5.
14
........ du!dt ....... Kp
qr ... l.+ , ..... + e t .... q ... ARM + . i+ .... .....
... .... Kd
Figure 3.1: PD control loop of the manipulator
3.1 PD Control
Numerous control methods such as adaptive control, neural network control, fuzzy control
have been studied. However, controllers in industrial and commercial robot manipulators
are still employing the clasical PID controllers because of their simplicity and ease of
implementation.
The block diagram of the PO controller is shown in Figure 3.1. The motor torque T for
the PD control law is given by
(3.1)
where e = qd  q is the tracking error, qd and q is the desired and actual angular position
respectively.
3.1.1 Simulation Results
This simulation shows that the robot tracks a desired trajectory, which is a circle starting
from point (O.58,O)m and ending at this point in a time interval of 4 seconds
PD controller is simulated with fixed parameters PI> pz and P3. Figure 3.2 to 3.4 show
the simulation results.
Figure 3.4 shows the simulation result with a step payload disturbance. The payload
IS
· ~

0.15
0.1
0.05
0
I
~
005
01
015
02
03 0.35 OA OA5
x(m)
0.5 O.SS oe
Figure 3.2: Trajectory in Cartesian coordinate
Angle (PO contrail
Angular Error(PO conlrol)
35
" ............ __._. _.
1.5 2.5
1.5
05
05
~~
  _. 
2
1.5
~g.
J
~ 05
<T
J' 0
cF
05
1
0
0.01
0.005
~g.
~ 0 w
t: w
0.005
001
0
Figure 3.3: Angular position and angular position error
16
· .    
Payload (PO conlroQ
50,,..,...,..._,
40
10
0.03.__,_,=~=;__,r,r,____,
0.02
I 0.01
~ 001
002
Figure 3.4: Tracking perfonnance in the presence of payload
variation results in the change of the dynamic parameter Pb P2 and P3 It can be seen that
the tracking error increases in the presence of the payload variation.
3.2 Model Based Adaptive Control
[n practice, there are uncertainties in the model parameters and nonideal efforts such
as friction, backslash, dead zones, etc. Adaptive control is effective in handling parameters
uncertainties. This section investigates the adaptive control designed for trajectory tracking
and gives the simulation results on a twolink planar manipulator.
Recall the dynamic model for the n degreeoffreedom robot:
M(q)q + C(q, q)q + g(q) + If = T (3.2)
where q, qE jRn is the generalized position and velocity vectors, respectively, M(q) is the
generalized mass matrix, C(q, q) is the matrix composed of Coriolis and centrifugal tenns,
g(q) is the gravi.ty vector, T E lRn is the vector of generalized input forces, II E Rn is the
vector of friction forces.
17
The adaptive control law is given by
(3.3)
where M(q), C(q, q), and g(q) are the estimates of M(q), C(q, q), and g(q), respectively.
Av is a positive definite gain matrix, and
ev = q  qr
e = q  qd
(3.4)
(3.5)
(3.6)
(3.7)
where Ap is a positive definite gain matrix. The reference velocity error ev is defined as
the difference between actual joint velocity q and reference velocity qr' Substituting the
control law (3.3) into the robot dynamics (3.2) yields
!'vf(q)ij + C(q, q)q + g(q) = M(q)ijr + C(q, q)qr + g(q)  Avev. (3.8)
Subtracting [M(q)ijr +C(q, q)qr +g(q)] from both sides of the above equation and simplifying,
M(q)(ij  ijr) + C(q,q)(q  qr) + Avev
= (M(q)  M(q))ijr + (C(q,q)  C(q,q))qr + g(q)  g(q)
Using the linear parameterization property yields
(3.9)
(M(q)  M(q»)ijr + (C(q, q)  C(q, q))qr + g(q)  g(q) = Y(q, q, qr) ijr)jj (3.10)
For the planar manipulator considered in this thesis, related parameters are shown in
the following.
 ~ (3=(3(3=
18
~PI  PI
~P2  P2
~P3  P3
(3.11)
Y( q, q., q.r, q..r ) = [ qrl
o Qrl + Qr2
cos(q2)(2qrl + Qr2)  Sin(q2)[q2qrl + (ql + qr2)qr2] ]
COS(q2)Qrl + sin(q2)qlqrl
(3.12)
Substituting (3.10) into (3.9) yields error dynamics, which is
(3.13)
Consider the following Lyapunov function candidate:
(3.14)
The time derivative of the Lyapunov function candidate along the trajectories of (3.13) is
'. T • 1 T . :T ~
V(t) =ev M(q)ev + 2cll M(q)ev + {3 f{3
 1· : ~
=e~[Y(q, q, qr, Qr){3  C(q, q)ev  Avev]+ 2e~ M(q)ev +(3Tf{3 (3.15)
1 . : ~ ~
=  e~ Avev + e~[2M(q)  C(q, q)]ev+ (3Tr{3 + e~Y(q, q, qr, qr){3
Since [~M(q)  C(q, q)] is a skewsymmetric matrix, e~a(M)(q)  C(q, q)lev = 0,
hence
(3.16)
If we let
then the time derivative of the estimated parameter error is given by
Ii rTYT(.. .. ) fJ =  q, q, qr, qr ev ·
  Since {3 is constant, {3 = (3, and hence,
f.i rTyT(.. .. ) fJ =  q, q, qr, qr ev ,
19
(3.17)
(3.18)
(3.19)
(3.20)
that is,
jj(t) = 730  itrTyT(q, q, qr) qr)evdu
where 7Jo is the initial value of jj(t).
Equation (3.20) gives the adaptation law for the parameters. Substituting Equation
(3.17) into Equation (3.16) and simplifying yields the following equation:
(3.21)
From Equation (3.14) and (3.21), it can been seen that V(t) is positive definite and V(t)
is negative definite, so V(t) is indeed a Lyapunov function. Because V(t) is a positive
function that is bounded from below, e" and f3 are bounded. Since the regressor matrix
y (q, q, qr) qr) in (3.13) remains bounded and manipulator inertia matrix M(q) is never
singular during the motion, ev is bounded from (3.13). This means that ev is unifonnly
continuous and hence converges to zero asymptotically.
3.3 Computed Torque Control
In some robotic applications, robot parameters are known. The computed torque is
commonly used to act as a feedforward part in the controller. This computed torque controller
is given by
T = M(q)qr + C(q, q)qr + g(q)  Avev (3.22)
where qn ev , e, qd, qd and Av are defined the same as in Section 3.2. Substituing the control
in Equation (3.22) into the robot dynamics in Equation (3.2) yields
M(q)q +C(q, q)q + g(q) = M(q)qr + C(q, q)qr + g(q)  Avev. (3.23)
The error dynamics is
(3.24)
20
Choose the Lyapunov function candidate
1
V(t) = 2"e;M(q)ev > O.
The time derivative of V(t) along the trajectories of error dynamics is
(3.25)
(3.26)
Thus, V is a Lyapunov function, i.e., V is positive definite and V is negative definite.
Hence, all the internal signals, e, ev , are bounded. Further, from (3.24) ev E .coca and from
(3.26) ev E L2. Therefore, ev , ev E Loo and ev E L2' we conclude that ev  0 as t  00.
Since ev = e+ Ape, we have e  0 as t  00.
3.4 Model Based Adaptive Control with Static Friction Compensation
The controllers designed in the previous sections do not include friction compensation.
High accuracy tracking in mechanical systems cannot be achieved if friction effects are
not considered properly in the dynamic model used for control. Failure to compensate
for friction in robotic applications may lead to large tracking errors and limit cycles when
velocity reversals in the trajectory are required.
Consider robot dynamics in (3.2), and the static friction model
i, = FJl + Fcsgn(q), (3.27)
where Fv is a diagonal matrix with diagonal elements representing static friction constants,
and similarly Fe is a diagonal matrix with diagonal elements representing Coulomb friction
constants for each link of the mechanical system.
The following adaptive control law is chosen for the robot based on the static friction
model:
(3.28)
21
 . ..  
where Fv and Fc are the estimates of Fv and FCI respectively, M(q), C(q, q) and g(q) are
the estimates of M(q), C(q, q), and g(q), respectively. Substituting the control law (3.28)
into the robot dynamics and simplifying yields the following error dynamics:
 
= Y(q, q, qT) qr)(3 + Y/s(qr)(3/s
(3.29)
Notice that since Fv and Fc are diagonal, the elements of the parameter vector (3/s are the
diagonal elements of Fv followed by the diagonal elements of Fc•
Stability of the solutions of the closedloop error dynamics (3.29) can be shown by
considering the following Lyapunov function candidate:
(3.30)
where rand r / s are diagonal gain matrices. The time derivative of the Lyapunov function
candidate along the trajectories of the error dynamics is given by
. T . 1 T . "":'T 1  ;J' 1 V = ev M(q)ev+ 2"ev M(q)ev + (3 r (3 +(3/sr/s (3js
= e~(Fv + Av)ev  e~ Fc(sgn(ev + qr)  sgn(qr))
+ (e~Y (q, q, qT) iir) +j3T r1)j3
+ (e~YJs(qr) + j3Jsrf})j3j8
where the skewsymmetry property of the matrix [~M(q)  C(q,q)] has been used in the
simplification.
22
Notice that
n
= L evdei (sgn(qi)  sgn(qri))
i=l
n
= L fei(qi  tin) (sgn(qi)  sgn(qri))
i=l
n
= L fei (qisgn(qi)  qisgn(qri) + qrisgn(qri)  qrisgn(qi))
i=l
n
= L fei (Iqil qisgn(qri) + Iqril qrisgn(qi))
i=l
;::::0
Further if we let
and since the parameter vectors (3 and (3f 8 are constant, the update law for the estimated
manipulator and static friction parameters are given by
As a result, the time derivative of the Lyapunov function candidate simplifies to
(3.31)
(3.32)
(3.33)
Thus, V is a Lyapunov function, Le., V is positive definite and V is negative definite.
~ ...
Hence, all the internal signals, e, ev , (3, (3/8' are bounded. Further, from (3.29) ev E Loo,
and from (3.33) ev E £2' Therefore, ev , ev E Loo and ev E £2, we conclude that ev ~ 0 as
t ~ 00. Since ev = e+ Ape, we have e ~ 0 as t ~ 00.
23
3.4.1 Simulation Results
The simulation of the proposed modelbased adaptive controller with static friction
compensation is conducted before this controller is applied to real plant. Figure 3.5 shows
the tracking error for the circle trajectory. Figure 3.6 and 3.7 show the estimated parameters.
The tracking error due to the payload introduced at time t = 2sec. at the endeffector
is shown in Figure 3.8.
Compared to the corresponding results shown in Figure 3.2, Figure 3.3 and Figure
3.4 of PD control, it can be observed that the proposed adaptive controller offers better
tracking error in the face of payload disturbance. Figure 3.8 and 3.4 show that the PO
controller with fixed P and D gains is very sensitive to the dynamics uncertainties caused
by the changing payload. However, by using adaptive controller, the tracking error increase
due to the introduction of the payload is much smaller than that in PO control.
Circle TraJedoty (Adaptive control)
c ~ 
0.15
0.1
0.05
E 0
"'0.05
<l.1
··0.15
<l.2
0.3 0.35 0.4 0.45
x (m)
Angular Error Signals
0.5 0.6
4
5 L_'__..L._l._"'__L_'__..L._'
o 0.5 1.5 2 2.5 3 3.5
t(sec)
Figure 3.5: Adaptive control + static friction compensation simulation result: position and
tracking error
24
Esbmated Paramet&rs (ACS)
1.8r,,,c,r,.,,
1.6
1.4
1.2
/
 Plhatj
  p2hat
 Dahal
0.6
0.4
0.2      . _.   _.    ."..  .........
2 2.5 3 3.5 4
t (sec)
0.5 1.5
O'__'__.l .J '__'__' I._'
o
Figure 3.6: Adaptive control + static friction compensation simulation result: estimated
parameters (fh, P2, fi3)
Eslimated Paramele,.. (ACS)
."..       '   
0.03
0.035 rr=~~:::;_.r,,.,.~
 Fv(I,'
  Fv(2,2
 Fe(I,'
• Fe 2,2
002
0005
J 0.01
~
u.
*!i 0.0'5
.t
N 0.025
oJ
.F
0.005''''''1...1'
o 05 '.5 2 2.5 3 3.5 4
I (sec)
Figure 3.7: Adaptive control + static friction compensation simulation result: estimated
parameters (Fv(1, 1), Fv(2, 2), Fe(1 , 1), Fe(2, 2))
25
Payload lACS)
50
40
~
~30
"0 ~'"20
0'"..
10
0a 0.5 1.5 2 2.5 3 3.5 4
• 10 J Angular Error(ACS)
1
05
U~
 0 '
~.5
r'f" 1
C' Err1
1.5
Err2
2
0 0.5 1.5 2 25 3 3.5 4
t (sec)
Figure 3.8: Adaptive control + static friction compensation simulation result: tracking
perfonnance in the presence of payload
3.5 Adaptive Control with Dynamic Friction Compensation
The classic static friction models cannot sufficiently describe all the dynamic effects of
friction, such as presliding, the friction lag and Stribeck effect. These phenomena all occur
in the low velocity and presliding region. To incorporate changing friction characteristics
with velocity and for designing efficient friction compensation techniques, several friction
compensations utilizing LuGre model are proposed ([11]  [16]). The basic concept is to
improve the tracking performance, where the unmeasurable friction state z is estimated by
an observer which is driven by the tracking error. Constant parameters in LuGre model are
identified offline. However the friction force varies with normal force, temperature and
other factors. This variation can be considered as the effect of parameter uncertainties. A
friction compensation algorithm with three unknown parameters {31,{32 and {33 was proposed
in [16]. This report will extend the research result and applied it to the multilink robot
26
application. The robot dynamics and friction dynamics derived in Section 2.6 are
M(q)q I C(q, q)q + g(q) + Q3q + QOZ  Q)WZ = T (3.34)
dz .
dt = q  WZ (3.35)
and
Consider the following control law
where zo and Zl are estimates of the internal state z in the LuGre model. Observers are
necessary because z is unmeasurable. The observer dynamics is given by
(3.36)
(3.37)
Substituting the control law into the robot dynamics and simplifying results in the following
closedloop error dynamics:
   I Q3qr + Qozo + QoZa  QI wz)  Ql wZj. (3.38)
where Oi = Qi  Qi and ~ = Zi  Zi. Since Qi is a diagonal matrix, define a column
vector ()i whose elements are the diagonal elements of Qi. Also, define
YOd(Za) = diag(zOl, ... , Zan),
27
where ZOi, Zli, Qri, i = 1 : n, represent the elements of the nvectors ZQ, ZI, and Qr, respectively.
Therefore, the error dynamics, (3.38), can be written as
~ ~ ~ + Y3d(Qr)03 + YOd(ZO)OO  \lIYId(zdOl
(3.39)
where Oi
~
Oi  Oi. Subtracting each of (3.36) and (3.37) from (3.35), we obtain the
observer error dynamics as
Consider the following Lyapunov function candidate:
(3.40)
(3.41)
where f, r 0, f 1 and f 3 are diagonal positive gain matrices. The time derivative of the
Lyapunov function candidate along the trajectories of the error dynamics is given by
. TIT' ;J' ~ ;J'
V = ev M(q)ev + "2ev M(q)ev + (3 fl(3 + Zo Qozo
;J' ;J'  ;J' _ ;J' ~
+ Zl QIZI + 00 faIOo+ 01 f I 101 + 03r a 103
Substituting the closedloop error dynamics and the observer error dynamics,
v= e~(Av + Q3)ev + (fl/f' I e~Y(q,Q,qr,qr))jj
 z;rQo \}Jzo + ~Qoev  e~Qoio
 Z[Q1\{fZI  Z[Ql tJlev +e~Q1 \lIzl
;J' ~
I (faIOo +e~YOd(ZO))OO
;J' ~
I (fIle 1  e~Y1d(Zl))Ol
;J' ~
+ (ra103 + e~Y3d(Qr))83
28
(3.43)
Since Qo, Q1 and 'l' are diagonal, ZJ'Qoev  e~Qozo = 0 and ZfQl'l'ev +e~QlwZI = O.
Further, if we let
:...J' r113 + e~Y (q, q, qr, Qr) = 0
;...T
f o1Qo + e~YOd(ZO) = 0
·T rl 1Ql  e~Yld(Zi) = a
·T
r31Q3 + e~Y3d(qr) = °
. . . .
and since 73 = fj and Oi = ~,i = 0, 1,3, the update laws for the estimated manipulator and
dynamic friction parameters are:
(3.44)
(3.45)
(3.46)
(3.47)
As a result, the time derivative of the Lyapunov function candidate simplifies to
(3.48)
Thus, V is a Lyapunov function. Hence, all the internal signals are bounded. Since
Au, Qo, Ql' Q3 and Ware positive definite matrices, using the same arguments as before
we conclude that e, ev , zo, ZI. fj, ea,~, Oa are bounded, and ev , e, ZO, ZI t 0 as t t 00.
3.5.1 Simulation Results
The simulation results of the dynamic controller with dynamic friction compensation
are shown in Figure 3.9 and 3.1 O.
Figure 3.9 shows the controlled angular position, velocity and the tracking error. It can
be seen that the tracking errors have peaks at the zerovelocity points. For joint 1, these
points take place at time t = 1.2sec and t = 2.25sec. For joint 2, it takes place at t = 2sec.
29

Around the low velocity area (q ~ 0), the controlled system exhibits larger tracking errors
than at high velocity. This is because the friction force has a quick jump with a directional
reverse at zerovelocity crossing point, and exhibits the Stribeck effect. As a result, tracking
error increases. At low velocity, the friction force has a lot of variation, whereas the friction
force is almost constant when the velocity is high enough. When friction compensation is
used, the controlled manipulator is forced to the desired position quickly. The estimated
parameters Qo, 01. Q3 are shown is Figure 3.10. These parameters converge to constant
values.
Angular pos~ion (ACO)
j 1 .
:. I        
CJ'""
,j 01__"' _
1.5 2 2.5 3 3.5
AngUlar velocity (ACO)
0.5
1 '__.l__'__'__'__'__'__'__'
o
2..,..,,..,....,.....,
4
 ....  
3.5
3 3.5
2.5 3
1.5 2 2.5
Angular Error Signals
1.5
Error (Qldq,
Error(q2d~
0.5
.. 1 1
:g 0 I~...,'~_
d'
.g 1 Ql dol
r:F Q2dol .
2 L...!===========!'__'__'__....I..__'__'__....J
o
x 10'
4..,..,,,,,.....,
~r; 2
CJ"
;"'0 r"D
r:F_2'JL.·L..l'''...J
o 0.5 1
Figure 3.9: Simulation: position, velocity and tracking error
30
·10'" Parameter estimate (ACD) 15,,,.,,..,...,...,..,
10
5 1
01(1.1)1
.    00(2,2)1
,
I
I
/
0~~~,.,.,;::""'1
0.5 1.5 2 2.5 3 3.5 4
_5''''l.'''' o
0.5 1
01(1,1)1
....    01(2,2)1
1.5 2 2,5 3 3,5 4
 _.  
. : 
1.5 2 2.5 3 3.5 4
I (sec)
OL.........:;~::....,L,::....;=__.L____.L____~.L____..L____.J..____~_____lo
0.5
.10 '
2r,,.,,..,...,..,..,
0.5
1.5
Figure 3.10: Simulation: estimated parameters
31
CHAPTER 4
Experimental Platform
lbis chapter gives a detailed description of the experimental platform, on which the
designed controllers are tested. The open architecture feature of the experimental platform
makes it easier for user to write various control algorithms and to introduce any additional
specific hardware (such as Forcefforque sensor, gripper) to the manipulator.
The experimental platform consists of a twoaxis direct drive manipulator as shown in
Figure 4.1. The direct drive manipulator operates in the absence of the undesirable factors
such as mechanical backlash and gear train compliance. It eliminates the need for gear
reduction, so repeatability is limited only by the resolution of the position feedback. Each
axis of the manipulator is driven by an NSK Megatorque direct drive servomotor.
I Link 2
[ Motor 2 Electronic E]=
Controller Interface
Link 1
HDriver l Motor 1
KDriver } f
r Robol Base
I Base Table I
Figure 4.1: Sketch of the robot system
32
I
Serial
Commands
Latch I
80386
HOST
.. "
Reference
Signals
Position Position
... I,;
Feedbac~krDS~P~S~ervo'IFeedback
Controller ~
Serial
Commands
I Latch I
Motor
Driver I
I !
Limit Position Actuation
Switches Feedback Signals
Motor
Driver 2
! I
Actuation Position Limit
Signals Feedback Switches
Figure 4.2: Hardware architecture
4.1 Hardware
Figure 4.2 shows the block diagram of the hardware architecture of the control workstation.
In standard operation, user interface, inverse kinematics and trajectory generation
(interpolation) take place on the 80386 based host computer, while servo control is implemented
on the Spectrum Signal Processing TMS320C30 OS? Processor Board. This
scheme takes advantage of the hardware floating point capability of the 80386/80387 system
and the computational power and efficient I/O handling of the TMS320C30 OSP. The
motor drivers perfonn digital communication to and from the direct drive motors, and can
optionally provide closed loop velocity control.
The NSKMegatorque motor system consists of motor and its driver unit. This is a
standalone system that contains all the elements needed for a complete closedloop servo
motor controL The NSK motor consists of a high torque direct drive brushless actuator,
a highresolution brushless resolver, and a heavy precision bearing. The servo motors
33

are capable of up to 3 revolutions per second maximum velocity and position feedback
resolution of up to 156,400 counts per resolution. The base motor delivers up to 245 Nm
of torque output, and the elbow motor produces up to 40 Nm torque output.
4.1.1 Motor Drivers
The motor driver consists of the following components:
• Power Amplifier(PA): Amplifies the controller output to a level which is capable of
driving the direct drive motors. AC power and the temperature of PA are monitored,
and corresponding steps are taken to protect PA from damage.
• Resolver Interface: Position and velocity feedback signals are provided by resolver
interface circuit.
• Digital Signal Processor: A 16 bit microprocessor system receives commands from
outside world in either analog form or digital fonn. The command parameters can
be position, velocity, or torque. The digital signal processor receives its feedback information
from the resolver interface(position) and limit switch signals. Many techniques
are applied in this part to improve the repeatability and eliminate mechanical
resonances, including
 Using a digital integrating function to improve the repeatability of the motors.
 To avoid mechanical resonances, digital notch filters is employed to cut out
certain frequencies.
 A digital lowpass filter is employed to modify motor frequency response and
make the motor smooth and quite.
 An RS232C serial port is provided for user to communicate with the digital
signal processor directly.
34
_u_~ K ITorqu~
Torque Mode
u + Torque Manipulator Veloci K \'j Kv Dynamics

ty
Velocity Mode
Figure 4.3: Torque mode vs. velocity mode
1\vo types of control modes(torque mode and velocity mode) selected by user are implemented
in the motor driver unit. Refering to Figure 4.3, it can be seen that the driver simply
behaves like a current amplifier, producing a motor torque which is proportional to the input
signal. In velocity mode, the amplifier provides closedloop velocity control. Closedloop
velocity control operates with a sample time of 550j1.s.
4.1.2 DSP Servo Controller
DSP Servo Controller is implemented by a Spectrum TMS32OC30 DSP Processor
Board. This board is installed on the bus of the host computer and interfaced to the motor
drivers via a DS2 motion control interface board. The user servo algorithm is executed in
DSP servo controller. The DSPIDS2 combination provides the TMS320C30 DSP, 128 KW
RAM, 2 01A converter, 2 AID converter, 2 shaft encoder interface and 4bit parallel I/O
(refer to Figure 4.4).
35
180386 HOST
Spectrum TMS32OC30 DSP Board . i
I, ,,
: DP RAM I<==~ 1MS320C30 I¢=~ONBOARD :
: DSP RAM:
, ' I
~ ,
2CH
AID
4 BIT
DIG I/O
PULSE e=
DECODE
DS2 Interface Board
,,,
~'
2CH
D/A
4 BIT
DIG I/O
I
I
I
I,,
t=:;:: ,,•I
I
.   ..
I,
I,,
,
r.::=f=
1  _
DRIVERS ROBOT
Figure 4.4: Connection diagram of the robot system
4.2 Software
4.2.1 RPL Software
An RPL(Robot Programming Language) program is a CIanguage program which is
created by the user through an editor, then compiled by executing the Compile RPL File
command. When the RPL program is executed, the execution of the controls software is
temporarily suspended, and the RPL program is spawned as a separate process which runs
on the host computer.
36

4.2.2 Servo Software
The servo software (control algorithm) is written in programming language C in the
host computer, then downloaded to the DSP board after compilation by the TMS32OC3OC
compiler. The user can monitor the desired signals when the control algorithm is running.
The run time executive module handles the communication between the host computer and
the servo DSP and delivers the output signals to the motor driver units via two DS2 UO
interface cards.
4.3 Manipulator Kinematics
y
/
/",q2b _
:~
/
/
/
/
/
/
/
/
/
/
(x,y)

Figure 4.5: Diagram of the work space of the manipulator
Figure 4.5 shows the diagram of the work space of the manipulator. The transformation
form joint space to work space is:
(4.1)
where ;J; and y are the work space coordinates of the end effector and qI and q2 are the base
angle and elbow angle, respectively.
37
Taking the derivative of (4.1) gives the work space velocity (x, iJ)T as a function of the
joint space velocity (b, (hf. which is
where
J(q) =
oh1 8hl
Oq1 OQ2
oh2 oh2
OQ1 8Q2
[
Llsin(ql)  L2sin(q1 + q2)
L1cos(ql) + L2cos(Q1 + q2)
(4.2)
(4.3)
is the Jacobian matrix of the manipulator. Since the determinant of J(q) is given by
(4.4)
(4.5)
It is obvious that the manipulator has singularity at any point where q2 = aor q2 = IT. Physically,
if a constant, nonzero work space velocity is desired as the manipulator approaches
singularity point, the joint space velocities required to achieve such a work space velocity
are given by
q=J1 j(
1 T .
det(J) co! (J)X
[
L2COS(ql +q2) L2sin(ql +q2) ]
L1cos(qd  L2cos(ql + q2) L1sin(q1)  L2sin(Q1 + q2) .
=..,..=X.
L1L2sin(q2)
Since dei( J) approaches zero as the manipulator approaches the singularity point. the required
joint space velocities become unbounded. As a result. during work space motions
the end effector should always be kept away from the singularity regions where r = L1  L2
or r = L] + L 2 •
Refering to Figure 4.5, the joint angles in terms of Cartesian coordinates are
. 1 x 2 + y2  Lr  L~
q2 = g(x, y) = cos ( 2£1£2 )
38
(4.6)
and
(4.7)
Equation (4.6) and (4.7) give the inverse kinematics of the manipulator, i.e.,
(4.8)
q= [::] IL2Sin[Cos1(X2 + y2  L~  L~)]I tan1(~) =F sin 1 (x2+ y2~1~iL2
± 1 (x2+ y2  L~  L~)
COS 2L
1
L
2
Notice that (4.6) has two distinct solutions q2a and q2b where q2a = q2b and an infinite
number of solutions 2n7fq2a,b. The transformation is not unique. Using equation (4.7) to
express q1 as a function of q2 and q' = tan1 (y / x) reveals that q2a and q2b each yield a
different value of q1. This is represented graphically in Figure 4.5. The dashed and solid
line represents elbowup and elbowdown configuration of the arms, respectively.
4.4 Manipulator Dynamics
y
Figure 4.6: Diagram of the twolink planar NSK manipulator
39
Figure 4.6 shows the diagram of the NSK manipulator, which is a direct drive manipulator.
Each axis of the manipulator is driven by an NSKMegatorque direct drive servo
motor.
h Motor 1 rotor inertia
M1 Motor 1 mass
12 Link 1 moment of inertia
M2 Link 1 mass
13 Motor 2 rotor inertia
M3 Motor 2 mass
he Motor 2 stator inertia
M4 Link 2 mass
14 Link: 2 moment of inertia
Mp Payload mass
Ip Payload moment of inertia
q) Angle of link 1 with respect to the horizontal
q2 Angle of link 2 with respect to link I
L) Length of link: 1
L2 Length of link 2
L3 Distance ofCO of link 1 from axis rotation
L4 Distance of CO of link 2 from axis of rotation
The parameters defined in Figure 4.6 are shown in the above table. The equations of
motion for the manipulator can be derived by using the EulerLagrange equations,
Tl Torque applied by motor I
T2 Torque applied by motor 2
lel Axis I friction force
Ic2 Axis 2 friction force
i = 1,2 (4.9)
d 8L 8L
dt·(8· )  8 = Qi,
qi (ji
40

where £, = K  'P, K and 'P are the kinetic and potential energy of the robot, respectively.
q is a vector of generalized coordinates which completely describe the configuration of the
manipulator and Q is a vector of generalized forces which are applied to the manipulator.
In this case,
q= l::1 (4.10)
lT]  Ifl 1 Q = T  If =
T2  If'}.
(4.11)
The dynamic equation for the twolink planar manipulator is:
A1(q)q + C(q,q)q = T(t) !J (4.12)
where
constructed.
Notice that there in no gravity term for NSK manipulator, duo to the way in which it is
(4.14)
(4.15)
(4.13)
(4.16)
P2 + P3COS (Q2) l'
P2
41
(2q] + (2)COS(q2)  qisin(q2) 1
(sin(q2) + COS(Q2))q]
M(q)q + C(q, q)q = Y(q, q, q)f3
Y(q,q.,.q.) = lq]
o
where
is a vector of Coriolis terms and
lPI +2p3COS(q2)
M(q) =
P2 +P3COS (q2)
is the state varying inertia matrix.
The left side of Equation (4.12) can be parameterized as


is the regressor matrix, and
is the system parameter vector, where
f3 = P2
P3
(4.17)
 42
(4.18)
(4.19)
(4.20)

CHAPTER 5
Experiments
5.1 Desired Trajectory Generation
In this section, a desired trajectory in Cartesian space is generated for simulation and
experiment. Suppose the desired trajectory is a circle with a radius of r and center at
(xc, Yc), as shown in Figure 5.1. The desired trajectory should have smooth joint space
trajectories such that qd(t) E .coo and C/d(t) E C3, i.e., qd(t), qd(t) and 'cid(t) E £00' In
y
x
Figure 5.1: Circle trajectory
Cartesian space, the coordinate of the endeffector:
Z
[
x(t) j [Xc + rcos(¢(t)) j
I y(t)  Yc + rsin(¢(t))
Let
(5.1)
43

with the initial condition
4>(0) = 0,4>(0) = 0,4)(0) = 0, ¢(0) = 0 (5.3)
and the end condition
(5.4)
where tf is the instant when the circle trajectory ends. From (5.2), we can get
Joint 1 Join! 2

2 3 4
Time, sec
a
~ 01 §:2.j. ~l·:E""""·.~·:.··"····""··""" _0.5'" ..... :..... . . : .
r:F •. t:i" 1 .... .. :... .. :...... . ....
1 0.5
a 1 2 3 4 a 1 2 3 4
i_:I:=m (I 2J\l/J 1 2 3 4 a 1 2 3 4
(II'~ lA .2\ZJ a 1 2 3 4 0 1 2 3 4 ij;;:r;J ij:;[lid 02340
Time, sec
Figure 5.2: Diagram of qd, qd, qd and T for a circle trajectory
44

GO
al
4>(t) 1 t t 2 t 3 t4 t S t6 t 7 a2
~(t) 0 1 2t 3t2 4t3 5t 4 6tS 7t6 a3 (5.5)
(i,(t) a 0 2 6t 12t2 2Ot3 3Ot4 42tS Q4
';p' (t) 0 0 0 6 24t 6Ot 2 120t3 210t4 as
116
a7
Substituting the initial and end conditions into (5.5) and solving the matrix equation results
in :>
1
ao 0 0 () 0 0 0 0 a > al 0 0 0 0 0 0 0 0 5,.
a2 0 0 2 0 0 U 0 0 (j "J)
~
a3 0 0 0 6 0 0 0 0 0 ~ a= (5.6)  I tf t 2 t 3 t 4 t S t 6 t 7 271"  Q1 f f f f f f
as 0 Ztf 3t} 4t} St} 6t~ 7t1 0
a6 0 0 2 6tf 12t2 ZOt3 30t} 42tS 0
f f f
a7 0 0 0 6 24tf 60t2 120t} 21Ot} 0
f
Figure 5.2 shows the openloop response of joint angule, velocity and acceleration, as well
as computed torque when a circle trajectory is generated. The circle is selected with a
radius of a,lm, center at (0.48, O)m. It can be seen that the joint angule, velocity, acceleration
and torque are smooth. Motor torques are within the range of the maximum allowed
(Tlmax=240Nm, T2max=40Nm),
5.2 Experimental Condition
All the five controllers (PO, AC, CT, ACS and ACD) designed in Section 3 were implemented
on the experimental platform described in Section 4. The tracking perfonnance of
45

the five controllers are compared. Two desired trajectories are chosen: (l) circle trajectory
introduced in Section 5.1, (2) sinusoidal trajectory of desired joint position for both links
of the manipulator. The amplitude of the sinusoidal trajectory is 0.8 radians and frequency
is 0.2 Hz. The sinusoidal trajectory tests the tracking performance of each controller for
low velocity «1 rad/s) and velocity reversal. The control sampling period is chosen to be
2 milliseconds. The payload on the endeffector is constant. Since the frequency of the
sinusoid is 0.2 Hz, the period of each cycle is 5 seconds. Six cycles of data was collected
for each experiment.
The following gain matrix are chosen in the experiments: Ap = diag(15, 15), Av
diag(100,25), f = diag(1,0.2,0.2), f js = diag(l, 1,1,1), fa = diag(100,25), f l
diag(100,25), f 3 = diag(100,25). The initial values of estimated parameters are set as
i3(0) = [1. 7,0.5, 0.5]T, i3/s(O) = [0,0,0, aV, Qi(O) =diag[O, 0], i = 0, 1,3.
To consistently compare the performance of different controllers, the common gain
parameters in different controllers are kept the same for all experiments. The robot is
running under PD control in the normal operation. A particular controller (CT, AC, ACS,
or ACD) can be chosen by pressing the assigned key on the keyboard.
5.3 Motor Characteristics
The motors consist of a high brushless actuator, a high resolution brushless resolver,
and a heavy duty precise NSK bearing. The high torque actuator eliminates the need for
gear reduction, while the builtin resolver usually makes feedback components, such as
encoders or tachometers unnecessary. Finally the heavy duty bearing eliminates the need
for separate mechanical support since the motor casing can very often support the load
directly in most applications. Figure 5.3 gives the free body diagram of a DC motor. If
the rotor and shaft are assumed to be rigid, the motor torque, T, is related to the armature
current, i, by a constant factor Kt . The back emf, e, is related to the rotational velocity by
46
>5
•

Figure 5.3: Free body diagram of DC motor
the following equations:
From the Figure 5.3 we can write the following equations based on Newton's law combined
with Kirchhoff's law:
J() +!J = T
di .
L dt + Ri = V  K () e
(5.7)
(5.8)
(5.9)
(5.10)
)
where
J: moment of inertia of the rotor
K t : mature constant
Ke : motor constant
R: electric resistance
L: electric inductance
V: source voltage
(); position of thaft
T: motor torque
if: friction torque
Applying Laplace Transfonn, the above equations can be expressed in terms of sand
eliminating I (s) we can get the following transfer function, where the rotating position is
47

the output and the voltage is the input.
From Equation (5.11), it can been seen that the motor dynamics depends on friction. If
only viscous friction is considered, i.e., ft = Fur), Equation (5.11) can rewritten as
O(s)
V(s)
(5.12)
For the two link manipulator, the friction parameters of each link are identified sepa
Considering different friction models results in different motor dynamics.
5.4 Parameter Identification of the LuGre Friction Model
rately. Recall that the LuGre friction model for each link is given by
dz. Iql
dt = q  fJo 9(q) z,
g(q) = Fe + (Fs  Fc )e(Q/ws )2,
and the motor dynamics is
lij=Tft·
(5.13)
(5.14)
(5.15)
(5.16)
)
>5
...... I>
f)
;!
~
We can identify friction parameters in Equation (5.13) and Equation (5.14) by following
steps.
• Identify Fe and fJ2
When q>> Wa , we can consider steadystate friction only, i.e,
(5.17)
Hence, the motor model becomes
(5.18)
48

Rewriting Equation (5.18) yields
where
(5.19)
and <p = [ ei j
sgn(q) .
(5.20)
Using the least square estimation, we get
(5.21)
where
¢ = [<P(I), "', <p(n)] and Y = [ y(I),
where n is the number of data points.
y(n) r, (5.22)
)
§...
Figure 5.4 and 5.5 show the experimental results for link 1 and link 2, respectively. A
sinusoidal toque is applied to each link. Only the data with the angular velocity leil > Ws
are taken into the estimation of 1'~ and {32. The experiments indicate that Fc for link 1 and
link 2 are 1.857 N m and 0.7805 Nm, respectively, and f32 are 1.9291 N.m.s/rad and
0.38 N.m.s/rad, respcctively.
• Identify Fs
Fs can be identified by the break away experiment, i.e., the torque is increased from a
small value and registered when the link just starts to move. Fs for link 1 and link 2 are 11
Nm and 1.6 Nm, respectively.
• Identify Stribeck velocity, w.•
When the link moves at sufficiently small velocity, the friction is given by
(5.23)
Equation (5.23) substituted into the motor dynamics results in
(5.24)
49

The linear parameterization on the unknown parameter Ws is
where,
and
y = q·2, f) = 2 1 WS ' <P = n'Y
'Y = (T  ..Jq") sgn (q.)  rDc •
(5.25)
(5.26)
(5.27)
The real solution for Ws only exists for I 2: 1. The experimental results for link 1 and link
2 are shown in Figure 5.6 and 5.7, respectively. The Stribeck velocities Ws for link 1 and
link 2 are 0.14 rad/s and 0.096 rad/s, respectively.
• Identify 130
When the motion is in the presliding region where elastic effects dominate over the
plastic effects, the friction is given by
>5
...... >f)
~
i1
II = f3oq.
Hence
where
y = T, f) = 130 and <p = q.
(5.28)
(5.29)
(5.30)
Figure 5.8 and 5.9 show the experiments for both links. The rest stiffness f30 for link: 1
and link: 2 are 341ON.m/rad and 838N.m/rad, respectively.
In the above identification procedures, the accuracy of the identified parameters depends
on the parameters which are assumed known. The inertia J and the constant v2t
between the applied voltage and generated torque by the motor are the major factors that
50

affect the identification results. Because tbe torque T generated by motor cannot be measured
directly in the experimental platfonn, it it computed by the applied voltage V from
the D/ A board using the relation T = v2c * V. Both J and v2c are selected as nominal
values in the experiments. However the actual J and v2c may be different from the nominal
values that are chosen, which win result in identification error. The joint position q,
velocity qand acceleration ij are assumed accurate. This is reasonable because of the high
resolution of the resolver which measures the joint position; q and ij are computed from
q using finite difference approximation. As a result, q and ij may be noisy. This disadvantage
can be overcome by applying a low pass filter to filter the high frequency noise.
The resolution of the resolver affects the accuracy of f30 as the experiment for identifying
it requires measuring very small joint position q. More accurate parameter identification
methods need further investigations with a very high resolution resolver. As the adaptive
control with dynamic friction compensation use some of the identified friction parameters,
the effect of inaccurate friction parameters on the closedloop system is also a problem
which requires future study.
5.5 Experimental Results
5.5.1 PD Control
The experimental results for PO control are shown in Figure 5.10. The experiment is
conducted without payload disturbance and with a fixed proportional and derivative gain.
The first column of Figure 5.10 shows the joint position, joint velocity and joint position
error for link: 1, and the second column shows results for link 2. From the figure it can be
observed that tracking errors are small without disturbance by using only PD controller.
Observe that the maximum tracking errors occurs at the points where the joint velocity
reverses direction, which is due to low velocity friction.
51
)
,. 5..

Angular velocity dql
10..~_._,...___,.___,
5 10 15 20 25 30
10 ' L ' ' ' " '
o
Applied torque 1:,
)
5.I.I.'
>
f)
~
Tt ~...
15 20 25 30
nme (sec)
5 10
_30'_~____'_ :.L__'______'>L__'________'_'__'____~____I. ~___'
o
20
Figure 5.4: Identification of {32 and Fe for link 1
5.__,.,,..,.,,
Angular velocity d~
r
~
:'"> 0
E!
~ 10
i~ 0
"8
Qi
>

10 15
Angular posl1lon q,
5
x 10'"
4r,,··
OL'_' ~ _.J
o
Applied torque ~1
10r,.,
)
5 10 15
OL«'~ _.J o
Figure 5.8: Identification of /30 for link 1
Time (sec)
2
8
The experimental results of the modelbased adaptive controller are shown in Figures
5.11 and 5.12. From the Figure 5.11, it can be seen that the tracking error is smaller than
that of PO control, especially for link 2. The tracking error of link 2 in adaptive control is
much smaller than in PD control. Figure 5.12 shows the estimated parameters Pl' fh and
5.5.2 Model Based Adaptive Control
~
Tn the experiment, the initial values of parameter estimate /3 are taken to be half of their
nominal value. The adaptive controller therefore starts as a PO controller with a feedforward
part. The feedforward part plays an important role as the parameter adaptation is
driven by the tracking error. Increasing the gain matrix Av will speed the tracking process;
Av in the adaptive controller takes the same role as gain Kp does in PD controller.
However, although the tracking error is guaranteed to converge to zero, the adaptation
law does not guarantee the parameters converging to their true values. In the absence
54

Angular IXlsition q2
X 103
2.5,,,,
5 10 15
oLl========_L _
o
0.5
2
u
g 1.5
co
'+'
.~ 1
ll.
Applied torque t 2
1.4,,,,
1.2
_1
E
~0.8
III
50.6 o
f' 0.4 )
5
Time (sec}
10 15
•
)..
Figure 5.9: Identification of 130 for link: 2
of friction, the condition for the estimated parameters asymptotically converging to the
true values is that the matrix Y(qd, qd, qd, qd.) is persistently exciting (PE) and unifonnly
continuous, i.e., there must exist positive constants 0, al and a2 such that for all t l 2: 0
(5.31)
This adaptive controller does not need the measurement of angular acceleration and the
use of the reference modeL This advantage makes it easy to be applied to an industrial
robot without much computation. The parameter adaptation process should be stopped if
the estimated parameters reach a priori known bounds.
5.5.3 Computed Torque Control
Figure 5.13 gives the experimental result for computed torque control. Comparing
Figures 5.13 and 5.11, it can been seen that the computed torque control and the adaptive
 55

)·
..
)•
..
Position (Urk 1)  PO Position (Urk 2)  PO
'S
~
§ c:
0
:~ 0.5
',=
.~ 1
a. Q.
1
0 5 10 15 20 25 5 10 15 20 25
Velocity (Urk 1)  PO Velocity (Urk 2)  PO
~
~ 1
~ 0 gg
2
4
5 10 15 20 25 0 5 10 15 20 25
Tracking error (Urk 1)  PO Tracking error (Unk 2)  PO
0.01 0.04
0.02
'S '0
~ ~
... W 0 Q; 0.01 Ql
0.02
0.02 0.04
0 5 10 15 20 25 0 5 10 15 20 25
Time (s) Time (s)
Figure 5.10: PD control experimental result (circle trajectory)
control achieve almost the same level of tracking error. Because the computed torque acts
as a feedfoIWard part, small PO gain can be achieved compared with the PD control. High
;;
•..
PD gain may generate oscillation which should be avoided.
5.5.4 Model Based Adaptive Control with Static Friction Compensation
Tracking performance of adaptive control with static friction compensation is shown in
Figure 5.14 and Figure 5.15. Figure 5.14 shows that the tracking errors decrease with time.
At the beginning of the first trajectory cycle, the tracking error is maximum at the zero
velocity crossing points. As the friction compensation takes effect, i.e., estimates of the
friction parameter matrix Fv and Fc take nonzero values, these peaks decrease and even
disappear in link 2. The estimated parameters Fv (l, I), Fv (2, 2), Fc(l, 1) and Fc (2, 2) are
shown in Figure 5.15.
56
)
25
25
10 1S 20 25
T1rne (8)
Position (Link 2)  AC
5 10 15 20
Vetoclty (Link 2)  AC
5
5 10 1S 20
Tl1lcklng error (Link 2)  AC
25
25
10 15 20 25
Tlme (I)
Position (Lklk 1)  AC
5 10 15 20
Veloclty (Link 1)  AC
5
o
5 10 15 20
Tl1lcklng error (Link 1)  AC
0,01 r~~_,
0,005
I
'E
'" 0,005
Figure 5.11: Adaptive control experimental result: velocity and tracking error (circle tra )•
..110
25
25

20
20
20
15
15
15
10
10
10
Estimated Parameter (Link 1) AC
5
5
5
jectory)
6
4
~
2
0
0
0,65
0.6
0."'0,55
0,5
045a
0,9
0,8
0.'"0,7
0,6
0.5  0
Time(s)
Figure 5.12: Adaptive control experimental result: estimated parameters $1, P2. Pa) (circle
trajectory)
57 
Position (Unk 1)  CT P08I1Ion (Unk 2)  CT
!c
c: 0
:~ 05
0
13 S 1
0. 0.
1
0 5 10 15 20 25 5 10 15 20 25
Velocity (Link 1)  CT Velocity (Link 2)  CT
2 co 1 1
~ 0
~
~ 1
4
5 10 15 20 25 0 5 10 15 20 25
Tracking error (Unk 1)  CT Tracking error (Unk 2)  CT
0.01 001
0.005 ~ S~ ~ 0
~.. ~.. {).005
5
0.01
10 15 20 25 0 5 10 15 20 25
Tlme (s) TIfIl8(s)
Figure 5.13: Computed torque control experimental result: velocity and tracking error
(circle trajectory)
)
~•,..
Po.~ion (Unk 2)  ACS
~
~2 ;
c: ~
11
0. ')
t 5 10 15 20 25
Velocity (Unk 2)  ACS
4L_~_~__~_~_.J
o 5 10 15 20 25
Tracking error (Unk 2)  ACS
0.01,.,
Pos~lon (Unk 1)  ACS
S
~
15
:~ 0.5
0
0.
1
0 5 10 15 20 25
Velocity (Unk 1)  ACS
2 co
~g, 1
I 0
~1
5 10 15 20 25
Tracklng error (Unk 1)  ACS
o
0,005
!
~.. 0.005
5 10 15
Tlme (s)
20 25
~
~
0.005
0.01 L_~ ~_J
o 5 10 15 20 25
Tlme (s)
Figure 5.14: Adaptive control + static friction compensation experimental result: velocity
and tracking error (circle trajectory)
58 
Estimated Parameter (Unk ') ACS
0.7
0.6
0.5
;:;:04
;;:·0.3
0.2
0.'
0
0 5 10 15 20 25
Estimated Parameter (link 1) ACS
0.5
0.7
0.6
0.5
,,!:O.4
N
;;:·0.3
0.2
0.1
0.7
0.6
0.5
NOA
!i
u."0.3
Estimated Parameter (link 2) ACS
5 10 15 20 25
Estimated Parameter (Link 2) ACS
5 10 15 20 25
Time(s)
5 10 15 20 25
TIme (s)
Figure 5.15: Adaptive control + static friction compensation experimental result: estimated
parameters (Fv (1, 1), Fv (2, 2), f~(l, ]), Fe(2, 2)) (circle trajectory)
5.5.5 Adaptive Control with Dynamic Friction Compensation
The experimental results for ACD are shown in Figure 5.16 to 5.20. The tracking
error shown in Figure 5.16. Figure 5.17 shows the estimated parameters PI. ih and P3.
Figure 5.18 and 5.19 show the estimated friction coefficients in LuGre friction model for
link 1 and link 2, respectively. The estimated internal state z is shown in Figure 5.20.
The same tracking performance pattern with ACS is observed from Figure 5.16, but the
tracking errors decrease much faster than in ACS control. This illustrates that the friction
compensation using a dynamic friction model is better than using a static friction model.
5.6 Comparison of Experimental Results
Figure 5.21 to 5.24 present the comparative results of five different controllers to track a
circle trajectory. Figure 5.21 and Figure 5.27 show the tracking errors for both links under
59
.)...~
') ....
>
;:
d
Posltlon (Unk 1) AGO Posldon (Unk 2)  AGO
Figure 5.16: Adaptive control with dynamic friction compensation experimental result:
velocity and tracking error (circle trajectory) ')
.... :
.)
..
•:
5 10 15 20 25 5 10 15 20 25
Velocfty (Link 1) AGO Velocky (Link 2) AGO
~
g. 1
~ 0
~ 1
4
5 10 15 20 25 0 5 10 15 20 25
Tracking error (Unk 1)  ACO Tracking arror (Unk 2) AGO
0.01 0.01
0.005 0.005
'0 g. I 0
f.. ~.. 0.005
0.01 0.01
0 5 10 15 20 25 0 5 10 15 20 25
nme(s) nme(s)
;;
:
Estimated Perameter (Unk 1) AGO
8
6 _~_ f\A,...~~.,!"L.
":4
2
0
0 5 10 15 ?O 25
0.6
0."tJ.5
0.4
0 5 10 15 20 25
0.7
0.65
0: 0.6
0.55
,....
0.5
~
0 5 10 15 20 25
Time (s)
Figure 5.17: Adaptive control with dynamic friction compensation experimental result:
estimated parameters (j3), P2, 153) (circle trajectory)
 60
«
Estimated dynamics frtdlon p8l8melers (link 1)  ACO
0.5
0 a 0
5 10 15 20
<l.5 C__ ~c__ ____'c__ ~c__ c__ _____'
o
5 10 15 20 25
o= ____'c__ '' '' '' _____'
o
1.5..,..
2..,,.,.,
.~
;::1
(J' 0 ~.//
5 10 15 20 25
1 c__ c__ '' '' '' '
o
nme (s)
Estimalad dynamic friction parameten; (Unk 2)  ACO
•,.
·)·
•:
5 10 15 20 25
N O.5
0'"° 0 rvr~~
<l.5''''''
o
Figure 5.18: Adaptive control with dynamic friction compensation experimental result:
estimated parameters (Qo(1, 1), 01 (1,1), Q3(1, 1)) (circle trajectory)
10
15......r:,,
AA~._./
o
5 10 15 20 25
5' '' '' '' ' '
o
20.......,
5 10 15 20 25
0""' '' '' '' '' '
o
5
15
ex
~10
a'"
Time(s)
Figure 5.19: Adaptive control with dynamic friction compensation experimental result:
estimated parameters (00(2,2), Q1 (2,2),03(2,2)) (circle trajectory)
 61
d
Estimated dynamic Irldlon IlVlerslale z (Link 1)  ACD
5 10 15 20 25
0.6
0.6,,,...,,,
0.4
Eslimated dynamic lrictlon Inner slate z (Link 2)  ACD
nme(s)
Figure 5.20: Adaptive control with dynamic friction compensation experimental result:
estimated parameters (Z(JI, Zll, Z02, Zi2) (circle trajectory)
four controllers. Figure 5.23 and Figure 5.24 give the joint errors of the first two cycles of
each controller, which are put sidebyside to compare the relative performance of different
controllers. The 2 norm of the tracking error is also shown. It can be observed from the
tracking error that the adaptive controller with dynamic friction compensation(ACD) is
•••
•..::
I
.i.·.
considerably superior in perfonnance to the other three controllers.
To further test the performance of ACD, another set of experiments, which use a sinusoid
as a desired trajectory are conducted. The desired position, velocity and acceleration
trajectories are shown in Figure 5.25. Figure 5.26 to 5.33 show the experimental results of
the four controllers.
Figure 5.26 and 5.27 show the tracking error for link 1 and link 2, respectively. It can
be seen that the tracking error using ACD is about 5 to 10 times smaller than the other
three controllers. Also, observe that the controller ACS performs better than the controllers
CT and AC. Notice that the tracking error for the three controllers, AC, CT, and ACS, is

62
d
high when the link is going through the zero velocity region; change in friction torque is
significant in this region. Although similar tendency can be observed for the ACS controller
but dynamic friction compensation seems to significantly account for the friction changes
and hence has much lower tracking errors.
The estimates of the static friction parameters are shown in Figure 5.28. Estimates of
the dynamic friction parameters for link 1 and link 2 are shown in Figure 5.29, Figure 5.30
and Figure 5.31, respectively. Observe that all the estimated friction parameters tend to
converge to a constant value as more number of cycles are implemented. The estimated
internal state ztries to track the angular position.
Figure 5.32 shows the position tracking error and the 2nonn of the position tracking
error for link 1. The 2nonn shown is for all six cycles of data. Similarly, Figure 5.33 shows
the position tracking error and the 2norm of the position tracking error for link 2. Notice
that for each link the 2norm of the tracking error with the ACD controller is considerably
smaller than the other controllers.
63
:. .. ,.
;;
,.
.' ..
<
Trackjng error in Unk 1
(:~
o 5 10 15 20
0.01
'C
~
t;00:
o 5 10 15 20
~ 00:
001o 5 10 15 20 I00:
001o 5 10 15 20
Time(s)
Figure 5.21: Comparison of trac~ng errors for link 1 (circle trajectory)
Tracking error in link 2
o 5 10 15 20
(:
002L
o 5 10 15 20
~ot~~
0.02
o 5 10 15 20
10:~~
0.02 ====.J
o 5 10 15 20
Time (s)
Figure 5.22: Comparison of tracking errors for link 2 (circle trajectory)
64
•
j•
i·
X 103 TrackIng error with dlfferen1 controllers (link 1)
8r
A
:"':c, rc=:T=~:::~.,,....,
Figure 5.23: Tracking error and its 2nonn for link 1 (circle trajectory)
.:
J,
l·
x10J Tracking error with different oontrollerll (Unk 2)
4
61__..:..A:.::C ~C..:...T__~A:::C~S__=A..:..:C:..:D=_l
2norm of error in Unk 2
0.5
AC CT
0.4
0.3
0.2
0.1
0
2 3 4
Figure 5.24: Tracking error and its 2norm for link 2 (circle trajectory)
65
cd
Desired joint position
15 20 25 30
Desired joint velocity
5 10
1 ' l. J ..J.. ' ..l. '
o
Ico
.+i>ii
8. 0.5
2r,.··r.,.....
10 15 20 25 30
Desired joint acceleration
5
_2'l.J..J...J'..l. '
o
15 20 25 30
TIme (5)
5 10
N~ 2r,.r,.,.....
II) 1 1
c ,g 0
lXl
Qj
0; 1
~
_2'"'..J...J'''
o
Figure 5.25: Desired trajectories for link 1 and 2 (Sine trajectory) )i
..
~
Tracking error in Link 1
! ":
001 o 5 10 15 20 25 30
! ":
001LY o 5 10 15 20 25 30 (::o 5 10 15 20 25 30
,oor=~=~~
0.01 o 5 10 15 20 25 30
Time(s)
Figure 5.26: Comparison of tracking errors for link 1 (Sine trajectory)
66
Tracking error in Link 2
(~:
o 5 10 15 20 25 30
001
U
~
b 00:o 5 10 15 20 25 30
0.01
~
~
~oo:~
o 5 10 15 20 25 30 !00:
001 o 5 10 15 20 25 30
Time(s)
Figure 5.27: Comparison of tracking errors for link 2 (Sine trajectory)
Estimated static Iricilan parameters (ACS)
~>:[
o 5 10 15 20 "t> 30
~]
o 5 10 15 20 25 30
~>0:C;;;;;' : J
o 5 10 15 20 "5 30
~"o:l~ 1
o 5 10 15 20 25 30
nme(s)
Figure 5.28: Estimated static friction parameters (Sine trajectory)
67
."
ji
II
II
I"
Estimated dynamic friction paramet9f1l (Unk 1) (ACO)
d'05
20 25 30
o" L L .l... ' ' ',
o 5 10 15
0
0
30
20
,., 10
a
0
5 10 15
15 20 25 30
TIme(s)
5 10
10 L.. ' L .l... ' ' '
o
~ ~ ~
Figure 5.29: Estimated dynamic friction parameters (Qo, QJ, Q3) for link: 1 (Sine trajectory)
Estimated dynamic friction parameters (Link 2) (ACO) 1
5 10 15 20 25 30
o" ' ' J. L. '' ...J
o
3,.,,,,,,....,
~ ~ ~
Figure 5.30: Estimated dynamic friction parameters (Qo, QJ, Q3) for link 2 (Sine trajectory)
68
Estimated dynamic friction state z (Link 1)  ACO
1
15 20 25 30
TIme(s)
5 10
1.5 L.. ' ' ' .L ' '
o
Estimated dynamic friction state z (Link 2)  ACO
1
15 20 25 30
nme(s)
5 10
15 L.. ' ' ' .L ' '
o
Figure 5.31: Adaptive control with dynamic friction compensation experimental result:
estimated parameters eZCll, Zll, Z02, Z12) (sine trajectory) ,
Tracking error with diHerent controllers (Link 1)
0.Q1 ,.,,,.,
O.01l.'.J~'''
2norm of error in Link 1
0.8 AC
Figure 5.32: Tracking error and its 2norm for link l(Sine trajectory)
69
..,.
AGO
Tracking error with dl11er9nl controllers (link 2)
AGS
o
2
4
X103
6r..~~
_8L'''L.l
2nonn 01 error In Unk 2
O.5r~
AC
0.4
0.3
0.2
O.t
o
AClJ
Figure 5.33: Tracking error and its 2norm for link 2 (Sine trajectory)
70
cd
CHAPTER 6
Summary and Future Work
6.1 Summary
Tracking performance of mechanical systems with friction was studied in this thesis.
As a special case, a robot is selected as the real plant. Modeling, control design, investigation
of friction model for mechanical systems were considered. The dynamic model of
robots performing trajectory tracking operations was formulated in an efficient and realistic
manner that can be readily used in industrial applications. Based on this dynamic model,
robust control algorithms with friction compensation were developed for robots perfonrung
high accuracy tracking operations. The control algorithms that were developed in this thesis
are robust to uncertain parameters of the robot and uncertain parameters of the friction
model. An important aspect that is studied in this thesis is to reduce the trajectory tracking
error for mechanical systems. The stability of the robot in the presence of friction and
uncertain parameters has been proved. In addition to the theoretical contributions towards
this problem, some realtime software improvement has been done on the openarchitecture
experimental platform. Extensive experiments were conducted on a twolink manipulator,
and experimental results verify the feasibility of proposed control designs with friction
compensation.
Five types of control designs are considered in the thesis. They are: (1) PD control
(PD); (2) computed torque control (Cl'); (3) model based adaptive control (AC); (4) model
based adaptive control with static friction compensation (ACS); and (5) model based adaptive
control with dynamic friction compensation (ACD). Control law of each control is
given in the thesis. Stability of the two adaptive controllers with friction compensation
71
(ACS and ACD) that were proposed was shown.
PD control is the most commonly used controller. It is simple and robust For applications
demanding high accuracy tracking performance, PO controller cannot achieve desired
result in the presence of dynamic uncertainties and friction.
Computed torque approach is well known in robotic applications. It consists of a forward
compensation part and a PD part. To use the computed torque approach, the dynamics
of the robot should be known, which sometimes is not possible.
Model based adaptive control is prevalent in robotics literature. Usually, smooth Cartesian
motions are planned a priori and the inverse kinematics is used to compute the desired
joint position, velocity, acceleration. This kind of controller is advantageous since it follows
the desired joint trajectories corresponding to the planned Cartesian motion, and it
does not require assumption or simplification such as local linearization, time invariance,
or decoupleddynamics. The other important advantage is it does not need the measurement
of the joint acceleration or inversion of the estimated inertia matrix.
The adaptive controller used in the experiment enjoys essentially the same level of robustness
to measurement noise and unmodeled dynamics as the PD controller, yet achieves
higher tracking accuracy. The parameter estimation should be stopped when the estimated
parameters reach an upper bound. Stopping the adaptation process does not affect the stability
of the closedloop system.
Friction is a complicated phenomenon, which exhibits coulomb friction, viscous friction
and nonlinearity at low velocity. The classic friction models cannot sufficiently describe
all the dynamic effects of friction, such as presliding, the friction lag and Stribeck
effect. These phenomena all occur in the low velocity and presliding region. Compared
to the Dahl model and the BlimanSorine model, the LuGre friction model includes more
dynamic effects of friction.
To decrease the friction effect, friction compensation is incorporated into the adaptive
control deign. Two types of friction models were used in the control design. They are
72
....
the static friction model, which considers Coulomb and viscous friction, and the Lugre
dynamic fiction model. The internal state (bristle displacement z) and three of the six
parameters in LuGre model were estimated.
To compare the performance of the proposed control designs in this thesis, experiments
were conducted on a twolink manipulator. The sampling and control period was chosen to
be 2 millisecond. All the five controllers (PD, CT, AC, ACS and ACD) were implemented
in realtime and the control algorithms were programmed in an efficient way such that
the tracking error, joint velocity, the estimated parameters and the control torque data were
obtained for offline analysis. To consistently compare controllers, the control gains that are
common to all the controllers were set to the same value for each controller implementation.
The initial values of the estimated parameters in the controllers were chosen as follows: (1)
the initial values of the robot dynamic parameters Pi, P2 and P3 in AC, ACS and ACD were
chosen as half of their nominal values; (2) the initial values of the Coulomb and the viscous
friction coefficients in ACS were chosen to be zero; (3) the initial values of the estimate
of the internal state z, the rest stiffness coefficient 130, the velocity dependent damping
coefficient (3] and the viscous friction coefficient (32 in ACO were all chosen to be zero.
To illustrate the performance of the proposed controllers to various types of trajectories,
in this thesis two different types of desired trajectories were considered; circle trajectory in
Cartesian space and sinusoidal trajectory in joint space. Experimental data for six cycles of
the trajectory was collected for each controller for comparison. The following observations
can be made based on the experimental data:
• CT, AC, ACS and ACD all achieved better tracking performance than PO control.
• AC controller achieved about the same tracking error as CT controller, but AC does
not require accurate knowledge of parameters of the dynamic model.
• ACS and ACD gave smaller tracking error than the other three controllers. This indicates
that friction compensation plays an important role in improving the tracking
73
performance. Furthermore, it can be observed from the tracking error that the adaptive
controller with dynamic friction compensation(ACD) is considerably superior in
performance to the other four controllers. For the sinusoidal trajectory, where the
robot motion is in the low velocity region and crosses the zero velocity point frequently,
ACD reduced the tracking error significantly. The tracking error using ACD
is about half of that of ACS and about onefifth of the other three controllers.
6.2 Future Work
In this work, control design with friction compensation was investigated for robotic
applications. Future work should focus on some potential improvements to the proposed
control algorithms and potential applications in other areas.
Model based adaptive control with dynamic friction compensation with application to
robot motion control was the focus in this thesis. However, friction in all mechanical
systems is known to exhibit similar behavior. Besides the robotic applications, the control
schemes developed in this thesis can be applied to many industrial applications such as web
handling systems, CNC machines, and excavators [23]. In web handling systems. the lateral
position of a web is usually controlled by steering guides. A steeri ng guide consists of a DC
motor and some mechanical structures. Friction affects the lateral position accuracy and
should be compensated. Another examples is CNC machining. It is well known that CNC
machining system usually consists of multiaxis machines and cutting tools. High accuracy
positioning cannot be achieved if the friction effects are not considered. Excavators are
typical manipulatorlike structures with four degrees of freedom. The identification of
friction parameters and the control design with friction compensation for excavator arms
can be conducted using the methods outlined in this thesis.
The LuGre friction model is characterized by six parameters (/30, (31, (32, Fe' Fs and ws)'
Currently, the identification of the parameters of the LuGre friction model is conducted via
several procedures. In each procedure, the LuGre friction model is simplified by either dif
74
cd
ferent velocity regions or nonlinear performance in consideration of the friction behavior.
These experiments for identifying the friction parameters are offline, timeconsuming, and
maybe impossible for some systems. Hence, the identification of the friction parameters in
LuGre friction model in one experiment which can be readily implemented is a challenging
task for the future. This task includes designing the experiment and the identification
algorithm for estimating all the friction parameters together.
From the adaptive control point of view, the control algorithm may avoid the requirement
of the actual parameters in the LuGre friction model. In the model based adaptive
control with dynamic friction compensation proposed in this thesis, three parameters (Fc ,
Fs and ws ) were identified offline. Future work should focus on designing an adaptive
controller with dynamic friction compensation with all six unknown friction parameters.
Hence designing a control algorithm with estimation of all the friction parameters online
will also be a challenging task for the future.
75
....
BffiLIOGRAPHY
[1] YD. Song, "Adaptive Motion Tracking Control of Robot Manipulators  Nonregressor
based Approach," Proc. ofIEEE International Conference on Robotics and
Automation, San Diego. Vol. 4, pp 3008  3031, May, 1994
[2] J.J.E. Slotine and W. Li, "Adaptive Manipulator Control: A Case Study," IEEE Trans.
on Automatic Control, Vol. AC33, No. II, pp. 995 1003, November, 1988.
[3J J.J.E. Slotine and W. Li, "On the Adaptive Control of Robot manipulators," Proc.
ASME Winter Annual Meeting, pp. 55  56, December, 1986.
[4] Y Stepanenko and c.y. Su, "Adaptive Motion Control of RigidLink ElectricallyDriven
Robot Manipulators," Proc. of IEEE International Conference on Robotics
and Automation, San Diego, USA, Vol. 1, pp. 630  635, May, 1994.
[5] J.B. Pomet and L. Praly, "Adaptive Nonlinear Regulation: Estimation from the Lyapunov
Equation," IEEE trans. Automation Control, Vol. 37, pp. 729  740, 1992.
[6] P. Tomei, "Robust Adaptive Friction Compensation for Tracking Control of Robots,"
Proc. ofthe 1999 IEEE International Conference on Control Application, Hawi, USA,
pp. 875  879, August, 1999.
[7] R. Colbaugh and H. Seraji, "Adaptive Tracking Control of Manipulators: Theory and
Experiments," Proc. ofIEEE International Conference on Robotics and Automation,
San Digeo, USA, Vol. 4, pp. 2992  2999, May, 1994.
76
[8] G. Niemeyer and lJ.E. Slotin, "Performance in Adaptive Manipulator Control," Proc.
of the 27th Conference on Decision and Control, Austin, USA, pp. 1585  1591, December,
1988.
[9] J.J. Alsina and N.S. Gethlot, "Adaptive Controller of Robotic Manipulators: a Modular
Approach," Proc. of the 34th Conference on Decision and Control, New Orleans,
USA, pp. 57  62, 1999.
[] 0] M. Gafvert, "Comparisons ofTwo Dynamic Friction Models," Proc. ofthe 1997 IEEE
international Conference on Control Applications, Hartford, CT, pp. 386 391,1997.
[11] C.c. Wit, H. Olsson, K.J. Astrom, and P. Lischinsky, "A New Model for Control of
Systems with Friction," IEEE Trans. on Automatic Control, Vol. 40, No.3, pp. 419425,
March, 1995.
l12J C.c. Wit, "Comments on A New Model for Control of Systems with Friction," IEEE
Trans. on Automatic Control, Vol. 43, No.8, pp. 1189  1190, August, 1998.
[13] C.C. Wit and P. Lischinsky, "Adaptive Friction Compensation with Partially Known
Dynamic Friction Model," Intemational Journal ofAdaptive Control and Signal Processing,
11:6580, 1997.
[14] N.E. Leonard and P.S. Krishnaprasad, "Adaptive Friction Compensation for Bidirectional
Lowvelocity Position Tracking," Proc. of the 31st IEEE Conference on
Decision and Control, Vol. 1, pp. 267  273, 1992.
[15) S. Cong, "Two Adaptive Friction Compensations for DC Servomotors," Proc. of the
IEEE International Conference on Industrial Technology, pp. 113  117, 1996.
[16] y. Tan and I. Kanellakopoulos, "Adaptive Nonlinear Friction Compensation with.
Parametric Uncertainties," Proc. of American Control Conference, San Diego, California,
pp. 2511  25]5, June, 1999.
77
[17] P.R. Pagilla and B. Yu, "A Stable Transition Controller for Constrained Robots," IEEE
Transactions on Mechatronics, Vol. 6, No. I, pp. 65  74, March, 2001.
[18] J.J.E. Slotine and W. Li, Applied Nonlinear Control. Prentice Hall, Englewood Cliffs,
NJ, 1991.
[19] S.S. Neo and M.J. Er, "Adaptive fuzzy control of robot manipulators Control Applications,"
Proceedings ofthe 4th IEEE Conference on Control Applications, pp. 724729,
1995
[20] M. Jansen and R. Eckmiller, "Globally Stable Neural Robot Control Capable Of Payload
Adaptation," Proc. of 1993 International Joint Conference on Neural Networks,
Nagoya, Japan, Vol. 1, pp' 639  642, 1993.
[21] S. Arimoto, S. Kawamura, and F. Miyazaki, "Bettering operation of dynamic systems
by learning: A new control theory for servomechanism or mechatronics systems,"
Proc. of the IEEE Conference on Decision and Control Including The Symposium on
Adaptive Processes, pp. 1064  1069, 1984.
[22] S. Arimoto, "Robustness of learning control for robot manipulators," Proc ofthe IEEE
Conference on Robotics and Automation, Vol. 3, pp. 1528  1533, May, 1990.
[23] S. Tafazoli, P.D. Lawrence, and S.B. Salcudean, "Identification of Inertial and Friction
Parameters for Excavator Arms," IEEE Transactions on Robotics and Automation,
Vol. 15, No.5, pp. 966  971, October, 1999.
78
APPENDIX A
Manipulator Application Software
A.I Servo.c
#include <cntrl.h>
#include <rnath.h>
#define PTS 2503
#define MAX_TAU1 200
#define MAX_TAU2 20
#define VVFF_larnap2 15.0
#define VVFF_larnai2 0.01
#define VVFF_Fv2 25.0
#define VVFF_beta32 0
#define VVFF_beta02 0
#define VVFF_beta12 G
#define VVFF_garnrna02 5.0
#define VVFF_garnma12 5.0
#define VVFF_garnrna32 5.0
#define VVFF_z02 0.0
#define VVFF_z12 0.0
#define VVFF_larnap1 15.0
#define VVFF larnai1 0.01
#define VVFF_Fvl 100.0
#define VVFF_beta31 0
#define VVFF betaOl 0
#define VVFF beta11 0
#define VVFF_garnma01 5.0
#define VVFF_garnma11 5.0
#define VVFF_garnma31 5.0
79
c
fdefine VVFF_zOl 0.0
fdefine VVFF_zl1 0.0
#define VVFFp1 1.7
fdefine VVFFp2 0.5
fdefine VVFFp3 0.5
#define VVFF_fl 0.0
#define VVFF_f2 0.0
#define VVFF_gl 0.0
fdefine VVFF_g2 0.0
fdefine VVFF_gAmmapl 1.0
fdefine VVFF_gAmmap2 .2
#define VVFF_gAmmap3 .2
#define VVFF_gAmmafl 1
#define VVFF_gAmmaf2 1
#define VVFF_gAmrnagl 1
fdefine VVFF_gAmmag2 1
float VF_tau2max=19;
float VF_taulmax=121;
float VF_csaveflag=99;
float VF_abs_errl=O;
float VF_abs_err2=0;
float abs_errl=O;
float abs_err2=0;
float tau2max=19;
float taulmax=121;
float c2r=0.00004091,t2c_l=8.3551,t2c_2=51.75;
float pi=3.1415927;
float v2c=204. 8;
float Ts=0.002;
float alpha=O.O;
float 11=14.5*0.0254,12=9.5*0.0254;
80
float c2=O,s2=O,c12=O,s12=O;
int iSave=O;
int iDisplay=O;
float taul=O,tau2=O;
float TF_output[Ox8000];
float TF_ql[PTS];
float TF_q2[PTS];
float TF_dql[PTS];
float TF_dq2[PTS];
float TF_ddql[PTS];
float TF_ddq2[PTS);
float pl=3.4, p2=O.4, p3=O.3;
float ft_data[3];
float kpl=85*24,kp2=l8*4;
float kdl=1.9*24,kd2=1.9*4;
float kil=O, ki2=O;
float VF_kpl=85*24,VF_kp2=18*4;
float VF_kdl=1.9*24,VF_kd2=1.9*4;
float VF_ul=O, VF_u2=O;
float VF_ql=O, VF_q2=O;
float VF_display=O;
float VF_controlflag=lO.O;
float VF_controlflagl=O;
float VF_csaveflagl=O;
iot cootrolflag=O;
/*PD*/
float VF_lamap2=15, VF_lamai2=O;
float VF_lamapl=20, VF_lamail=O;
float VF_Fvl=VVFF_Fvl;
float VF_Fv2=VVFF_Fv2;
81
float VF_beta32=O, VF_beta02=O, VF_beta12=O;
float VF_gamma02=S,VF_gamma12=5,VF_gamma32=5;
float Fc2=O.780S, Fs2=1.6;
float inv_vs2=(1.O/O.096l*(1.0/0.096};
float VF_z02=O, VF_z12=O, varphi2=O;
float VF_beta31=O, VF_betaOl=O, VF_betall=O;
float VF_gammaOl=S,VF_gammall=S,VF_gamma31=5;
float Fcl=1.857, Fsl=ll,
float inv_vsl=(1.O/O.14)*{1.O/O.14);
float VF_zOl=O, VF_zll=O, varphil=O;
float oldpl=O,oldp2=O,oldp3=O,oldfl=O;
float oldf2=O,oldgl=O,oldg2=O;
float VFpl=1.7,VFp2=O.5,VFp3=O.5;
float VF_fl=O.O,VF_f2=O.O;
float VF_gl=O.O,VF_g2=O.O;
float VF_gAmmapl=1,VF_gAmmap2=.2,VF_gAmmap3=.2;
float VF_gAmmaf1=O, VF_gAmmaf2=O;
float VF_gAmmagl=O,VF_gAmmag2=O;
int VI_PTS=PTS; /* # of pts in a circle*/
int pts=O;
float VF_CYCNUM=O.O; /*# of circle*/
float serrl=O,serr2=O,rate_fv=1.0;
float errl=O,err2=O,olderrl=O,olderr2=O;
float derrl=O,derr2=O;
float evl=O,ev2=O;
int i=O,j=O; /* counters or index */
unsigned long k=O; /* index for saving data*/
float rl=O,r2=O;
float qql=O,qq2=O;
float qld,q2d,dqld,dq2d,ddqld,ddq2d;
82
float dqlr=O,dq2r=0,ddqlr=O,ddq2r=O,oldq:=O;
float oldq2=O,dql=O,dq2=O,olddql=O,olddq2=O,ddql=O,ddq2=O;
float tmpl=O,tmp2=O,tmp3=O;
float Ypll=O,Yp12=O,Yp13=O,Yp21=O,Yp22=O,Yp23=O;
init_control()
i=O;
k=O;
VI_PTS=1300;
Ts=O.002;
rl=O;
r2=O;
User_Data [0] =0;
VF_CYCNUM=l ;
serrl=O;
serr2=O;
control ()
qql=(float)posl;qq2=(float)pos2;
kpl =VF_kpl;
kp2=VF_kp2;
kdl=VF_kdl;
kd2=VF_kd2;
controlflag=(int)VF_controlflag;
pts=VI_PTS;
tau2max=VF_tau2max;
taulmax=VF_taulmax;
/* inactive the robot */
83
if (controlflag==lOO)
i=O;
k=O;
u1=0;
u2=0;
refl=pos1;
ref2=pos2;
Hast_Trigger=O.O;
gata m1;
}
else if (contralflag==98)
i=O;
k=O;
u1=0;
u2=0;
refl=pas1;
ref2=pas2;
Host_Trigger=O.O;
goto m1 i
else if (controlflag==97)
i=O;
k=O;
u1=0;
u2=0;
refl=O;
ref2=O;
Host_Trigger=O.Oi
gate ml;
else if (controlflag==99)
84

VF_lamai2=VVFF_lamai2;
VF_Fv2=VVFF_Fv2;
VF_beta32=VVFF_beta32;
VF_beta02=VVFF_beta02;
VF_betaI2=VVFF_betaI2;
VF_gamma02=VVFF_gamma02;
VF_gammaI2=VVFF_gamma12;
VF_gamma32=VVFF_gamma32;
VF_z02=VVFF_z02;
VF_z12=VVFF_zI2;
VF_lamapl=VVFF_lamap1;
VF_lamai1=VVFF_lamail;
VF_Fvl=VVFF_Fvl;
VF_beta31=VVFF_beta31;
VF_betaOl=VVFF_beta01;
VF_bet a l1=VVFF_betall;
VF_gammaOl=VVFF_gammaOl;
VF_gammal 1=VVFF_gamma1 1;
VF_gamma31=VVFF_gamma31;
VF_zO l=VVFF_zOl;
VF_z1I=VVFF_zII;
VFp1=VVFFpI;
VFp2=VVFFp2;
VFp3=VVFFp3;
VF_fl=VVFF_fl;
VF_f2=VVFF_f2;
VF_gl=VVFF_gl;
VF_g2=VVFF_g2;
VF_gAmmapl=VVFF_gAmmapl;
VF_gAmmap2=VVFF_gAmmap2;
VF_gAmmap3=VVFF_gAmmap3;
VF_gAmmafl=VVFF_gAmmafl;
VF_gAmmaf2=VVFF_gAmmaf2;
VF_gAmmagl=VVFF_gAmmag1;
85
VF_controlflag=O;
goto ml;
/* home the robot */
else if (controlflag==7l
olderrl=errl; olderr2=err2;
errl=c2r*qqlVF_ql;
err2=c2r*qq2VF_q2;
derrl=(errlolderrll/Ts;
derr2=(err2olderr2l/Ts;
serrl+=errl*Ts;serr2+=err2*Ts;
dql=c2r*(qqloldqll/Ts;
dq2=c2r*(qq2oldq2l/Ts;
ddql=c2r* (dqlolddqll/Ts;
ddq2=c2r*(dq2olddq2l/Ts;
oldql=qql;oldq2=qq2;
olddql=dql;olddq2=dq2;
tmpl=(kpl*errl+kdl*derrl+kil*serrll;
tmp2=(kp2*err2+kd2*derr2+ki2*serr2l;
if (tmp2>tau2maxltmp2=tau2max;
else if (tmp2<tau2maxltmp2=tau2max;
if (tmpl>taulmaxltmpl=taulmax;
86
else if (tmpl<taulmax)tmpl=taulmax;
ul= (int) (tmpl *t2c_l);
u2= (int) (tmp2*t2c_2);
goto ml;
if (Host_Trigger==l.O)
if(i<pts)
qld=TF_ql[i];
q2d=TF_q2[i];
dqld=TF_dql[i];
dq2d=TF_dq2[i];
ddqld=TF_ddql[i];
ddq2d=TF_ddq2[i];
oldcrrl=errl; olderr2=err2;
errl=c2r*qqlqld;
err2=c2r*qq2q2d;
derrl=(errlolderrl)/Ts;
derr2=(err2o1derr2)/Ts;
serrl+=errl*Ts;serr2+=err2*Ts;
abs_errl=(errl>O)?abs_errl+errl:abs_errlerrl;
abs_err2=(err2>O)?abs_err2+err2:abs_err2err2;
dql=c2r*(qqloldqll/Ts;
dq2=c2r*(qq2oldq2)/Ts;
oldql=qql;oldq2=qq2;
87
rate_fv=l.O;
c2=cos(qq2*c2r);s2=sin(qq2*c2r);
tmp2=lOO.O;
if (controlflag==lO.O)
/ *pd_part () ; */
taul=(kpl*errl+kdl*derrl);
tau2=(kp2*err2+kd2*derr2);
else if(controlflag==l.O)
Commonpart ();
adaptive_part();
taul=(Ypll*VFp1+Yp12*VF_p2
+Yp13*VFp3 )VF_Fvl*rate_fv*evl;
if(dqlr>O)taul+=VF_fl*dqlr+VF_gl;
else
taul+=VF_fl*dqlrVF_gl;
tau2=(Yp22*VFp2+Yp23*VF_p3)
VF_Fv2*rate_fv*ev2;
if(dq2r>O)tau2+=VF_f2*dq2r+VF_g2;
else
else if(controlflag==4.0)
Commonpart ();
adaptive_part();
taul=(Ypll*VFp1+Yp12*VF_p2
+Yp13*VFp3 )VF_Fvl*rate_fv*evl;
tau2=(Yp22*VFp2+Yp23*VFp3)
VF_Fv2*rate_fv*ev2;
88
else if(controlflag==2.0)
Commonpart ();
LuGrepart();
adaptive_parte);
taul=(Ypll*VFpl+Yp12*VFp2+Yp13*VFp3)
VF_Fvl*rate_fv*evl;
taul=taul+VF_beta31*dql+VF_betaOl*VF_zOl
varphil*VF_betall*VF_zll;
tau2=(Yp22*VFp2+Yp23*VF_p3)
VF_Fv2*rate_fv*ev2;
tau2=tau2+VF_beta32*dq2+VF_beta02*VF_z02
varphi2*VF_beta12*VF_z12;
else if(controlflag==8.0)
Comrnonpart ();
LuGrepart() ;
tau2=Yp22*p2+Yp23*p3VF_Fv2*ev2+VF_beta32*dq2
+VF_beta02*VF_z02varphi2*VF_beta12*VF_z12;
taul=ddqlr*pl+ddq2r*p2+Yp13*p3  VF_Fvl*evl
+VF_beta31*dql+VF_betaOl*VF_zOl
varphil*VF_betall*VF_zll;
else if (controlflag==3. 0)
Commonpart();
tau2=Yp22*p2+Yp23*p3 VF_Fv2*ev2;
taul=ddqlr*pl+ddq2r*p2+Yp13*p3 VF_Fvl*evl;
else
taul=O.O;
tau2=0.O;
89
taul={taul>taulmax)?taulmax:taul;
taul=(taul<taulmax)?taulmax:taul;
tau2=(tau2>tau2max)?tau2max:tau2;
tau2=(tau2<tau2max)?tau2max:tau2;
ul= (int) (taul *t2c_l) ;
u2= (int) (tau2*t2c_2);
rl=qql;
r2=qq2;
VF_CYCNUM;
VF_abs_errl=abs_errl;
VF_abs_err2=abs_err2;
abs_errl=O;
abs_err2=O;
iSave=(int)VF_csaveflag;
if (iSave==100)
iSave=(int)VF_controlflagl;
VF_controlflag=iSave;
k=O;
VF_CYCNUM=7 ;
iSave=(int)VF_csaveflagl;
VF_csaveflag=iSave;
if(VF_CYCNUM>O)i=l;
else
i=VI_PTS+lO;
90
i++;
if{k<{unsigned long)Ox07ffO)savedata{);
else
olderrl=errl;
olderr2=err2;
errl=c2r* (float) (qqlrl);
err2=c2r* (float) (qq2r2);
derrl=(errlolderrl)/Ts;
derr2=(err2olderr2)/Ts;
serrl+=errl*Ts;serr2+=err2*Ts;
oldql=qql;oldq2=qq2;
tmpl={kpl*errl+kdl*derrl+kil*serrl) ;
tmp2=(kp2*err2+kd2*derr2+ki2*serr2) ;
tmpl={tmpl>taulmax)?taulmax:tmpl;
tmpl=(tmpl<taulmax)?taulmax:tmpl;
tmp2=(lmp2>tau2max)?tau2max:tmp2;
tmp2=(tmp2<tau2max)?tau2max:tmp2;
u1= (int) (tmpl*t2c_l);
u2= (int) (tmp2*t2c_2);
else /*if{Host_Trigger==O.O)*/
i=O;k=O;
olderrl=errl;
olderr2=err2;
errl=c2r* (float) (qqlrefl);
91
err2=c2r* (float) (qq2ref2);
derrl=(errlolderrl)/Ts;
derr2=(err2olderr2)/Ts;
serr1+=errl*Ts;serr2+=err2*Ts;
oldq1=qql;oldq2=qq2;
tmpl=(kpl*err1+kdl*derrl+kil*serrl);
tmp2=(kp2*err2+kd2*derr2+ki2*serr2);
tmpl=(tmpl>tau1max)?tau1max:tmpl;
tmpl=(tmpl<taulmax)?taulmax:tmp1;
tmp2=(tmp2>tau2max)?tau2max:tmp2;
tmp2=(tmp2<tau2max)?tau2max:tmp2;
u1= (int) (tmp1 *t2c_1) ;
u2=(int) (tmp2*t2c_2);
m1:
display();
savedata ()
/*int iSave;
iSave=(int)VF_csaveflag;*/
switch (iSave)
case 0:
TF_output[k]=errl; k++;
TF_output[k]=err2; k++;
TF_output[k]=dq1; k++;
TF_output[k]=dq2; k++;
TF_output[k]=u1; k++;
TF_output[k]=u2; k++;
92
TF_output[k]=VF_zOl;
TF_output[k]=VF_z02;
TF_output[k]=VF_beta02;
TF_output[k]=VF_beta12;
TF_output[k]=VF_beta32;
TF_output[k]=VF_betaOl;
TF_output[k]=VF_betall;
TF_output[k]=VF_beta31;
break;
case 1:
TF_output[kl=dql;
TF_output[k]=dq2;
break;
case 2:
TF_output[k]=VF_beta02;
TF_output[k]=VF_beta12;
TF_output[k]=VF_beta32;
break;
case 3:
TF_output[k]=ddql;
TF_output[k]=ddq2;
break;
case 4:
TF_output[k]=VF_betaOl;
TF_output[k]=VF_betal1;
TF_output[k]=VF_beta31;
break;
case 5:
TF_output[k]=err1;
TF_output[k]=err2;
break;
case 6:
TF_output[k)=err1;
TF_output[k]=err2;
TF_output[k]=dq1;
TF_output[k]=dq2;
93
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++ ;
k+ f;
k++;
k++;
TF_output[k]=u1;
TF_output[k)=u2;
TF_output[k]=VFp1 ;
TF_output[k]=VFp2 ;
TF_output[k]=VFp3 ;
TF_output[k]=VF_f1;
TF_output[k]=VF_f2;
TF_output[k]=VF_g1;
TF_output[k]=VF_g2;
break;
case 7:
TF_output[k]=VFp1 ;
TF_output[k]=VFp2 ;
TF_output[k]=VFp3;
break;
case 8:
TF_output[k]=VF_f1;
TF_output[k]=VF_f2;
break;
case 9:
TF_output[k]=VF_g1;
TF_output[k]=VF_g2;
break;
case 10:
TF_output[k]=VF_f1;
TF_output[k]=VF_g1;
break;
case 11:
TF_output[k]=VF_f2;
TF_output[k]=VF_g2;
break;
case 12:
TF_output[k]=VF_beta02;
TF_output[k]=VF_beta:2;
break;
case 13:
94
k++;
k++;
k++;
k++i
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
TF_output[k]=VF_beta32;
TF_output[k]=VF_betaOl;
break;
case 14:
TF_output[k]=VF_betall;
TF_output[k]=VF_beta31;
break;
case 15:
TF_output[k]=VFp1 ;
TF_output[k]=VFp2 ;
break;
case 16:
TF_output[k]=VFp2 ;
TF_output[k]=VFp3 ;
break;
case 17:
TF_output[k]=VF_zOl;
TF_output[k]=VF_zll;
break;
case 18:
TF_output[k]=VF_z02;
TF_output[k]=VF_z12;
break;
default:
break;
display (l
/*int iDisplay;*/
iDisplay=(intlVF_display;
switch (iDisplayl
case 0:
Userl=(floatlerrl*50;
95
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
k++;
User2=(float}err2*5C;
break;
case 1:
Userl=ul/t2c_l;
User2=u2/t2c_2;
break;
case 2:
Userl=VF_z02;
User2=VF_z12;
break;
case 3:
Userl=VF_zOl;
User2=VF_zll;
break;
case 4:
Userl=VF_beta02;
User2=VF_beta12;
break;
case 5:
Userl=VF_beta02;
User2=VF_beta32;
break;
case 6:
Userl=VF_beta12;
User2=VF_beta32;
break;
case 7:
Userl=VF_betaOl;
User2=VF_betall;
break;
case 8:
Userl=VF_betaOl;
User2=VF_beta31;
break;
case 9:
Userl=VF_betall;
96
User2=VF_beta31;
break;
case 10:
Userl=VFp1 ;
User2=VFp2;
break;
case 11:
Userl=VFp2 ;
User2=VF_p3;
break;
case 12:
Userl=VFpI;
User2=VFp3;
break;
case 13:
Userl=VF_f1;
User2=VF_f2;
break;
case 14:
Userl=VF_gl;
User2=VF_g2;
break;
case 15:
Userl=VF_fl;
User2=VF_gl;
break;
case 16:
Userl=VF_f2;
User2=VF_g2;
break;
case 17:
Userl=qld;
User2=q2dj
break;
case 18:
Userl=dqld;
97
User2=dq2d;
break;
case 19:
User1=ddq1d;
User2=ddq2d;
break;
default:
break;
pdpart ()
tau1=(kp1*errl+kd1*derrl);
tau2=(kp2*err2+kd2*derr2);
ul= (int) ((kp1*err1+kd1*derr1) *t2c_1);
u2= (int) ((kp2*err2+kd2*derr2) *t2c_2);
Commonpart ()
dq1r=dq1drate_fv*(VF_larnap1*errl+VF_larnai1*serr1);
dq2r=dq2drate_fv*(VF_larnap2*err2+VF_larnai2*serr2);
ddqlr=ddq1drate_fv*(VF_larnapl*derr1+VF_larnail*err1);
ddq2r=ddq2drate_fv*(VF_larnap2*derr2+VF_larnai2*err2);
ev1=dq1dq1r;
ev2=dq2dq2r;
Yp13=c2*(2.0*ddq1r~ddq2r)(dq2*dqlr+(dq1+dq2)*dq2r)*s2;
Ypl1=ddqlr; Yp12=ddq2r;
98
Yp21=O; Yp22=ddqlr+ddq2r; Yp23=c2*ddqlr+s2*dql*dqlr;
varphil=fabs(dql)/(Fcl+(FSlFcl)*exp(dql*dql*inv_vsl));
VF_zOl=VF_zOl+(dqlvarphil*VF_zOlevl)*Ts;
VF_zll=VF_zll+(dqlvarphil*(VF_zllevl))*Ts;
VF_betaOl=VF_betaOl(VF_gammaOl*evl*VF_zOl)*Ts;
VF_betall=VF_betall+(VF_gammall*evl*varphil*VF_zll)*Ts;
VF_beta31=VF_beta31(VF_gamma31*evl*dqlr)*Ts;
varphi2=fabs(dq2)j (Fc2+(Fs2Fc2) *exp(dq2*dq2*inv_vs2) );
VF_z02=VF_z02+(dq2varphi2*VF_z02ev2)*Ts;
VF_z12=VF_z12+(dq2varphi2*(VF_z12ev2))*Ts;
VF_beta02=VF_beta02(VF_gamma02*ev2*VF_z02)*Ts;
VF_beta12=VF_beta12+(VF_gamma12*ev2*varphi2*VF_z12)*Ts;
VF_beta32=VF_beta32(VF_gamma32*ev2*dq2r)*Ts;
adaptivepart ()
tmp3=VF_gAmmapl*Ypll*evl*Ts;
VFp1=VF_pltmp3;
tmp3=VF_gAmmap2*(Yp12*evl+Yp22*ev2)*Ts;
VFp2=VFp2tmp3;
tmp3=VF_gAmmap3*(Yp13*evl+Yp23*ev2)*Ts;
VFp3=VFp3tmp3;
tmp3=VF_gAmmafl*dqlr*evl*Ts;
VF_fl=VF_fltmp3;
99
tmp3=VF_gAmmaf2*dq2r*ev2*Ts;
VF_f2=VF_f2tmp3;
if(dq1r>O)tmp3=VF_gAmmagl*ev1*Ts;
else
tmp3=VF_gAmmag1*evl*Ts;
VF_gl=VF_gltmp3;
if (dq2r>O)tmp3=VF_gAmmag2*ev2*Ts;
else
tmp3=VF_gAmmag2*ev2*Ts;
A.2 Rpl.c
#include "RPL.H"
#include "math.h"
#include "address.c"
#include "conio.h"
#define CYC_NUM 1
#define PI2 6.2831854
#define P5000 2501
extern void far PromptString(char far*,char far *,char far*);
char cNL=Ox20;
float ft_data[6];
int ft_ready=O;
float FREQl=0.5, AMP1=O.6;
float FREQ2=O.5, AMP2=O.6;
float c2r=O.00004091,t2c_1=8.3551,t2c_2=51.75;
float pi=3.1415927;
100
float v2c=204. 8; /*volts to D/A coun conv.*/
,
int nPts=2000; /* 2000;*/
int nStayPts=OO; /* stay at one point for nStayPts*Ts time */
int CYC_NUM_forsave=O;
float 11=14.5*.0254; /*=14.5 inch
float 12=9.5*.0254; /*=9.5; inch
length of base arm*/
length of the elbow*/
/* check it after compiling the corresponding servo program */
unsigned long addr_ft=Ox00030126; /* address of ft_data */
unsigned long li,lj;
float c2n=8,c2nm=8;
float uuul=0,uuu2=0;
char *str1=lr
counts to Newton and Newton Meter */
" .,
int ptnum;
/*float aa1[300],aa2[300],aa3[300];*/
float VFcontrolflag=O;
float tf_input;
float tmp;
float tmp1, tmp2;
float rr=5*.0254;
float xxO=19*.0254, yyO=O;
float ftmp [80];
float VFu1=0,VFu2=0,VFfreq=0,VFexp=0;
double lasttheta1,lasttheta2;
#include <dos.h>
101
#define FALSE
#define TRUE
#define Ts 0.002
o
1
L
unsigned addr_reg = Ox280;
unsigned data_reg Ox282;
unsigned status_reg Ox284;
unsigned ir<L.num 7;
unsigned data available FALSE;
unsigned interrupt_mode FALSE;
int send_command(unsigned *bptr, int numwords)
unsigned dword, command, ret code, loop
command = *bptr;
outpw(addr_reg,O);
for (;numwords;numwords)
dword = *bptr ;
outpw {data_reg, dword)
bptr ++ ;
outpw(addr_reg,Ox3FE);
inpw(data_reg);
outpw(addr_reg,Ox3FE)
outpw{data_reg,command);
outpw{addr_reg,Ox3ff) ;
outpw (data_reg, command)
ret code = TRUE ;
for{loop=TRUE;loop;)
102
dword inpw(status_reg);
if«dword & Ox0010)==O)
outpw{addr~reg,Ox3FE)
retcode = inpw{data_reg)
if(retcode & OxOOff) loop=FALSE;
return (retcode);
void receive_command{unsigned *bptr, int numword)
unsigned dword;
outpw{addr_reg,O);
for (;numword;numword)
dword
*bptr
bptr ++
struct
unsigned
unsigned
unsigned
unsigned
signed
signed
signed
inpw (data_reg) ;
dword
cmd_rc;
mode;
mat r i x [ 9 6] ;
sbias[8];
Mm[8];
Bm [8];
Mb [8];
103
signed
unsigned
unsigned
unsigned
unsigned
unsigned
unsigned
unsigned
unsigned
unsigned
} sensor_data;
Bb (8) ;
number_of_gages;
countsper_force;
countsper_torque;
max_force;
max_torque;
maxtemperature;
min_temperature;
units;
serial_number;
int read_sensor_info(void)
sensor_data.cmd_rc Ox0100
sensor_data.mode OxOOOO
if(!send_command(&sensor_data.cmd_rc,2)) return(FALSE)i
receive_command(&sensor_data.cmd_rc,sizeof(sensor_data));
c2n=sensor_data.countsper_force*O.1;
c2nrn=sensor_data.countsper_torque*O.1;
return (TRUE);
st ruct
unsigned
unsigned
unsigned
unsigned
unsigned
unsigned
unsigned
unsigned
unsigned
} card_data;
cmd_rc;
mode;
Data_C_flag;
Data_Mask;
Enable_Inter;
Sample_Rate;
Card_Model;
Ucode;
serial_number;
104
int change_card_data(void)
card_data.cmd_rc Ox0200
card_data.mode OxOOOO
if(!send_comrnand(&card_data.cmd_rc,2)) return(FALSE);
receive_command(&card_data.cmd_rc,sizeof(card_data) )
card_data.Sample_Rate=250;
card_data.Data_C_flag
= (card_data.Data_C_flag&Oxfff8) IOx0003; /* set filter */
card_data.cmd_rc=Ox0200;
card_data.mode=Ox0001;
if(!send_comrnand(&card_data.cmd_rc,sizeof(card_data)) )
return(FALSE);
return (TRUE);
struct
unsigned
unsigned
signed
} bias_array;
cmd_rc;
mode;
Bias_Array[8];
int change_bias_array(void)
bias_array.cmd_rc Ox0300
bias_array.mode OxOOOl
if(!send_comrnand(&bias_array.cmd_rc,2)) return(FALSE);
return (TRUE);
int read_ftpoll(void)
105
int loop, i;
int dword
int force_counts [3], torque_counts[3];
outpw(addr_reg,OxlOO);
dword = inpw(data_reg);
if(!dword) return(O);
dword = inpw(data_reg);
if (dword)
outpw(addr_reg,OxlOl);
outpw(data_reg,OxOO) ;
outpw(addr_reg,Ox010l);
outpw(data_reg,OxOO);
for(i=O;i<3;i++) force_counts[i] inpw(data_reg);
for(i=O;i<3;itt) torque_counts [i]= inpw(data_reg);
outpw(addr_reg,OxlOO);
outpw(data_reg,OxOO);
for(i=O;i<3;i++)
ft_data[i]=force_counts[iJ/c2n;
ft_data[i+3]=torque_counts[iJ/c2nm;
return(l);
Upload_Tablel(char *astr)
FILE *fn, *fnl;
float tfl,tf2,tf3,tf4,tf5,tf6,tf7,tf8,tf9;
106
float tf10,tf11,tf12,tf13,tf14,tf15,tf16;
int i, j;
fn=fopen("c:\\robot\\zhu\\aaaref.c","w");
if(*astr==O)fn1=fopen("c:\\robot\\zhu\\aaact.c","w");
else
fnl=fopen(astr,"w");
printf("%c[save to: %s] %c",13,astr,cNL);
printf("%c(# of data: %g] %c",13,
(float)CYC_NUM_forsave*(nPts+nStayPts),cNL);
if«fnl!=NULL)&&(fn!=NULL))
lj=addr_TF_output;
for(li=O;li«unsigned 10ng)6*(nPts1);li++)
/*lj=li*(unsigned 10ng)13;*/
if(li>Ox7fff)break;
tmpl=GetFloat(addr_VF_csaveflag,DUAL);
if(tmpl==O.O)
tfl=GetFloat«unsigned 10ng)lj,DUAL);
Ij+=(unsigned 10ng)1;/*zdelay(); errl*/
tf2=GetFloat«unsigned 10ng)lj,DUAL);
Ij+=(unsigned 10ng)1;/*zdelay(); err2*/
tf3=GetFloat«unsigned 10ng)lj,DUAL);
Ij+=(unsigned 10ng)1;/*zdelay(); dql*/
tf4=GetFloat((unsigned 10ng)lj,DUAL);
Ij+=(unsigned 10ng)1;/*zdelay(); dq2*/
tf5=GetFloat«unsigned 10ng)lj,DUAL);
Ij+=(unsigned 10ng)1;/*zdelay(); ul*/
tf6=GetFloat «unsigned long) 1j, DUAL);
Ij+=(unsigned long)l;/*zdelay(); u2*/
tf7=GetFloat«unsigned 10ng)lj,DUAL);
Ij+=(unsigned 10ng)1;/*zdelay(); z01*/
107
tfB=GetFloat«unsigned long)lj,OUAL);
Ij+=(unsigned long)l;l*zdelay(); z02*1
tf9=GetFloat(unsigned long)lj,OUAL);
lj+=(unsigned long)l;l*zdelay(); beta02*1
tflO=GetFloat«unsigned long)lj,OUAL);
lj+=(unsigned long)l;l*zdelay(); betal2*/
tfll=GetFloat«unsigned long)lj,OUAL);
Ij+=(unsigned long)l;/*zdelay(); beta32*/
tf12=GetFloat«unsigned long)lj,OUAL);
lj+=(unsigned long)l;/*zdelay(); beta01*/
tf13=GetFloat«unsigned long)lj,OUAL);
Ij+=(unsigned long)l;/*zdelay(); betal1*/
if(tmpl==O.O)
tfl4=GetFloat«unsigned long)lj,OUAL) i
lj+=(unsigned long)l;/*zdelay(); beta31*/
fprintf(fnl,"%g %g %g %g %g %g %g %g %g %g %g %g
%g %g %g\n", (float)li*Ts,tfl,tf2,tf3,tf4,tfS,tf6,
tf7/t2c_l,tfB/t2c_2/tf9,tflO,tfll,tf12,tf13,tf14);
else
fprintf(fnl,"%g %g %g %g %g %g %g %g %g %g %g %g
%g %g\n", (float)li*Ts,tfl,tf2,tf3,tf4,tfS,tf6,
tf7/t2c_l,tf8/t2c_2,tf9/tflO,tfll,tfl2/tf13) ;
else if«tmpl==l.O) II (tmpl==3.0) II (tmpl==S.O) II
«tmp1>7. 0) && (tmp1<18 .1)) )
tfl=GetFloat«unsigned long)lj,OUAL);
lj+=(unsigned long)1;/*zde1ay(); err2*/
tf2=GetFloat«(unsigned long)lj,OUAL);
Ij+=(unsigned long)l;/*zdelay(); dq2*/
fprintf(fnl, "%g %g %g\n", (float) li*Ts,tfl,tf2);
else if «tmpl==2. 0) I I (tmpl==4. 0) I I (tmpl==7. 0))
108
tfl=GetFloat«(unsigned long)lj,DUAL);
lj+=(unsigned long)l;
tf2=GetFloat(unsigned long)lj,DUAL);
Ij+=(unsigned long)l;
tf3=GetFloat(unsigned long)lj,DUAL);
lj+=(unsigned long) I;
fprintf (fnl, "%g %g %g %g\n", (float) li*Ts, tfl, tf2, tf3);
for(i=O;i«nPts+nStayPts);i++)
tfl=GetFloat(addr_TF_ql+i,DUAL);
tf2=GetFloat(addr_TF_q2+i,DUAL);
tf3=GetFloat(addr_TF_dql+i,DUAL);
tf4=GetFloat(addr_TF_dq2+i,DUAL);
tfS=GetFloat(addr_TF_ddql+i,DUAL);
tf6=GetFloat(addr_TF_ddq2+i,DUAL);
fprintf(fn,"%g %g %g %g %g %g %g\n",
(float)i*Ts,tfl,tf2,tf3,tf4,tfS,tf6);
fclose(fn);
fclose(fnl);
refl=posl;
ref2=pos2;
109
/* Define which to be plotted in real time */
draw_base=O;
draw_elbow=O;
draw_base_ref=O;
draw_elbow_ref=O;
draw_errl=O;
draw_err2=O;
draw_base_vel=l;
draw_elbow_vel=l;
draw_userl=l;
draw_user2=O;
set_display_range
(double xmin,double xmax,double ymin, double ymax)
x_min=xmin;
x_max=xmax;
y_min=ymin;
y_max=ymax;
delay(int i)
while(i)
i=il;
i=i/l;
i=i/l;
int zWait_For(seconds)
110
double seconds;
float aa,bb,cc;
float aal,bbl,ccl;
double final_time;
char oldcom = 0;
setpoints(l);
if (Simulation == l)init_simu(param_setl, cntrl_varl);
_disable();
final_time timer + seconds;
_enable();
for(;;)
_disable();
if (Simulation
art () ;
$command
aal=GetFloat(addr_VFp1,DUAL);
bbl=GetFloat(addr_VFp2 ,DUAL);
ccl=GetFloat(addr_VFp3,DUAL) ;
aa=GetFloat(addr_VF_abs_errl,DUAL);
bb=GetFloat(addr_VF_abs_err2,DUAL);
cc=GetFloat(addr_VF_CYCNUM,DUAL) ;
if(seconds<O.O)printf("%c %g %d[pl:% 6.4f p2:
% 6.4f p3:% 6.4f el:% 6.4f e2:%6.4f] %c",13,cc,
$command,aal,bbl,ccl,aa,bb,cNL);
if ($ command == 91) /* left windows key to exit*/
PutFloat(addr_VF_controlflag, DUAL, (float)lOO);
Slave_Trigger(O.O);
refl=posl;ref2=pos2;
111
bye () ;
else if ($command==92) I*stay at current point*1
PutFloat(addr_VF_controlflag,DUAL, (float)98);
Slave_Trigger(O.O);
refl=posl;ref2=pos2;
bye () ;
else if ($command==93) I*goto zero point*1
refl=O; ref2=O;
PutFloat (addr_VF_control flag, DUAL, (float) 97) ;
Slave_Trigger(O.O);
refl=O;ref2=O;
bye();
else if ($command==88) I*F12 save data*1
1* aa=GetFloat(addr_VF_csaveflag,DUAL);
delay(10);
PutFloat (addr_VF_controlflagl, DUAL, (float) (aa) ) ;
delay(lO) ;*1
PutFloat(addr_VF_csaveflag,DUAL, (float) (100));
else if«$command>58)&&($command<69))
PutFloat(addr_VF_controlflag,DUAL,
(float) ($command58));
if(seconds==7.0) 1*&&VFcontrolflag==7.0) *1
refl=posl;ref2=pos2i
if($command==92)goto zwaitli
112
zwaitl:
if((fabs(tmp2)<O.04)
&&(fabs(tmpl)<O.05 ))goto zwaitl;
if($command)oldcom = $command;
if«timer >= final_time)&&(seconds>=O.O))break;
if«seconds==l.O)&&(cc<=O))break;
_enable () ;
_enable();
if (oldcom) $command
else return(l);
oldcom;
/* back to (VF_ql, VF_q2) */
PutFloat(addr_VF_controlflag,DUAL,7);
zWait_For(7.0);
zWait_For(l.O);
set_current_ref();
float al,a2,a3,a4,a5,a6;
int i;
FILE *fn;
_disable();
fn=fopen("c:\\robot\\zhu\\traj.tra","r") ;
113
for (i=O; i<P5000;i++)
if(!feof(fn))
fscanf(fn,"%g%g%g%g%g%g",&al,&a2,&a3,&a4,&a5,&a6);
PutFloat(addr_TF_ql+i,DUAL,al);
PutFloat(addr_TF_q2+i,DUAL,a2);
PutFloat(addr_TF_dql+i,DUAL,a3);
PutFloat(addr_TF_dq2+i,DUAL,a4);
PutFloat(addr_TF_ddql+i,DUAL,a5);
PutFloat (addr_TF_ddq2+i,DUAL, a6) ;
fclose (fn) ;
_enable();
read_data_from_file()
FILE *fn;
_disable () ;
fn=fopen("c:\\robot\\zhu\\aaa.c","r");
fscanf(fn,"%f%f%f%f%f%f%s",&1,&FREQ1,
&2,&FREQ2,&tmpl,&tmp2,strl);
printf("%c[Ampl:%g Freql:%g Amp2:%g
Freq2:%g ctr:%g save:%g saveto:%s] %c",13,
AMPl,FREQl,AMP2,FREQ2,tmpl,tmp2,strl,cNL);
fclose(fn);
_enable () ;
PutFloat(addr_VF_controlflagl,DUAL,tmpl);
PutFloat(addr_VF_csaveflagl,DUAL,tmp2);
Download_sin ( )
FILE * fn;
int i;
float tt;
114
float al=0,a2=O,aldot=O,a2dot=O,a12dot=O,a22dot=Oi
zWait_for(2.0};
if (FREQl<FREQ2)tmpl=FREQ1i
else
tmpl=FREQ2 i
nPts=l.O/(tmpl*Ts);
_disable () ;
fn=fopen("c:\\robot\\zhu\\traj.tra","w"};
/*for(i=Oii<nPts+nStayPtsji++}*/
for(i=0;i<P5000ji++}
/* for (i=Oi i<5000; i++) */
tt=(float}i*Tsj
if(i<nPts)
al=AMP1*sin(PI2*FREQ1*tt)j
a2=AMP2*sin(PI2*FREQ2*tt) ;
aldot=PI2*FREQ1*AMPl*cos(PI2*FREQl*tt)j
a2dot=PI2*FREQ2*AMP2*cos(PI2*FREQ2*tt};
a12dot=PI2*FREQ1*AMPl*PI2*FREQl*sin(PI2*FREQl*tt) ;
a22dot=PI2*FREQ2*AMP2*PI2*FREQ2*sin(PI2*FREQ2*tt}j
fprintf(fn,"%g %g %g %g %g %g \n",
al,a2,aldot,a2dot,a12dot,a22dot};
fclose (fn) j
_enable () i
readparameter ()
char *stral="
char *stra2="
115
" .,
" .,
unsigned long lil,li2;
FILE *fn;
_disable () ;
fn=fopen("c:\\robot\\zhu\\aaact.c","w");
fclose (fn);
fn=fopen("c:\\robot\\zhu\\addresss.c"," r ");
while ( ! feof (fn»
fscanf(fn,"%s",stral);
if(strchr(stral,'_'»)strcpy(stra2,stral);
if(strstr(stral,"="»)
fseek(fn,3,SEEK_CUR);
fscanf(fn,"%G4x",&lil);
fscanf(fn,"%04x",&li2);
lil=(lil«16)+(li2&Gxffff);
fscanf (fn, "%S", stral);
if(!feof(fn) )
fscanf(fn,"%s",stral);
if(!strstr(stral,"unsigned")
trnpl=atof(stral);
printf("%c [%s=%g ]%c",13,stra2,trnpl,cNL);
PutFloat(lil,DUAL,trnpl);
zWait_for (G.l);
fclose(fn);
116
_enable () ;
test ()
Slave_Trigger(O.O);
Torque_Mode();
PutFloat(addr_VF_controlflag,DUAL, (float)99);
zWait_for(O.l);
gopoint()i
readparameter();
read_data_from_file();
/* download_sin();*/
Putlnt(addr_VI_PTS,DUAL,nPts+nStayPts);
tmp=GetFloat(addr_VF_CYCNUM,DUAL)i
if(tmp<l)tmp=CYC_NUMi
CYC_NUM_forsave=(int)tmp;
PutFloat(addr_VF_CYCNUM,DUAL,tmp)i
Slave_Trigger(l.O);
zWait_For(l.O);
set_current_ref();
Slave_Trigger(O.O); Wait_For(.5);
117
RPL ()
float stepa;
float x,y;
float theta;
int i;
int j;
/* Define which to be plotted in real time */
/* initiation */
Sample_Time((double)Ts);
test () ;
_disable();
Upload_Tablel(strl);
_enable();
118
APPENDIXB
MATLAB Programs
B.l PdditJ.m
function QDOT=pddiff(t,p) global 11 global 12 global tf global r
xc yc global phic dphic ddphic global numi global tempdata global
ttt global kp kd global pI p2 p3 global ppi pp2 pp3
q= (p (l) ; p ( 2) ) ;
dq= (p (3) ; P (4) ] ;
if t>tf/2
pl=ppl;
p2=pp2;
p3=pp3;
end
%angle
%angular velocities
ql=p(1);q2=p(2);dql=p(3);dq2=p(4);
phi=phic*[1;t;t~2;t~3;t~4;t~5;tA6;tA7];
dphi=dphic*[1;2*t;3*l A2;4*t~3;5*t~4;6*t~5;7*tA6];
ddphi=ddphic*[2;6*t;12*tA2;20*t~3;30*tA4;42*t~5];
x=xc+r*cos(phi); y=yc+r*sin(phi); dx=r*sin(phi)*dphi;
dy=r*cos(phi)*dphi; ddx=r*sin(phi)*ddphir*cos(phi)*dphi*dphi;
ddy=r*cos(phi)*ddphir*sin(phi)*dphi*dphi;
%plot (t, x, , b' , t, y, , r / , t / dx, / g' , t, dy, / b / , t, ddx, , r' , t, ddy , , g' ) ;
%% plot the desired trajectory
%subplot(2,1,1);
%plot (x,y,' ');
119
r
d=(x~2+y~211~2122)/(2*11*12); q2d=acos(d); qld=atan(y/x)atan(
(12*sin(q2d))/(11+12*cos(q2d)) )i
qd=[qld;q2d];
cl=cos(qld);c2=cos(q2d) ;c12=cos(qld+q2d)
;sl=sin(qld)is 2=sin(q2d);s12=sin(q1d+q2d);
jl=[12*c12 12*s12]; j2=[11*cl12*c12 11*sl12*s12];
xv=[dx;dy];
jinv=[jl;j2]/(11*12*s2);
dqd=jinv*xv; dqld=dqd(l); dq2d=dqd(2);
ddqld=(11*c2*dq2d*(c12*dx+s12*dy)11*s2*(s12*dx*(dqld+dq2d)
+c12*ddx+c12*dy*(qld+q2d)+s12*ddy))/(11 h 2*s2 h 2);
ddq2d=(11*12*c2*dq2d*((11*c1+12*c12)*dx+(11*sl+12*s12)*dy)
11*12*s2*((11*cl+l2*c12)*ddx+(11*sl*dqld
12*s12*(dqld+dq2d)) *dx+(11*sl+12*s12) *ddy
+(11*cl*dqld+12*c12* (dqld+dq2d)) *dy) )/((11*12*s2)~2);
ddqd=[ddq1d;ddq2d];
%%%%%%%%%% m and c matrix
m=[pl+2*p3*cos(q2) p2+p3*cos(q2);p2+p3*cos(q2) p2];
c=(p3*dq2*sin(q2) p3*(dql+dq2)*sin(q2); p3*dq1*sin(q2) 0];
%%%%%%% Control
e=qdq;de=dqddq; u=kp*e+kd*de;
%%%%%%% state space equation
qdot=dq; dqdot=inv(m)*(uc*dq);
120
%if ttt=t
ttt=t;
numl=numl+l;
tempdata(nurnl,l)=t;
tempdata(numl,2)=dqdot(1);tempdata(num1,3)=dqdot(2);
tempdata(numl,4)=u(1);tempdata(numl,5)=u(2);
%end
%%% plot the angular error
%subplot(2,1,2);plot(t,q(1)+q1d,'' ,t,q(2)+q2d,'')
%%%%%%% function return
QDOT=(qdot;dqdot];
B.2 Pdcontrolload.m
global 11 global 12 global tf global r xc yc global phic dphic
ddphic global num1 global tempdata global ttt global kp kd global
p1 p2 p3 global ppl pp2 pp3
ttt=l; %used for synchronize the time in diff equ.
11=14.5*.0254; %.38;
12=9.5*.0254; %.24;
r=4*.0254;xc=19*.0254;yc=0; p1=3.4;p2=.4;p3=.3;
mp=50;ppl=p1+mp*(11~2+12~2);pp2=p2+mp*12~2;
pp3=p3+mp*11*12;%payload =50(N)
kp=[1200 0;0 600]; kd=(100 0;0 30]; tf=4;
11=.38; 12=.24; r=.1;xc=0.48;yc=O;
num1=O;
A=(l a 0 0 0 a 0 0;
0 1 0 0 0 0 a 0;
0 a 2 a 0 0 a 0;
0 a a 6 0 0 0 0;
121
1 tf tf~2 tf~3 tf~4 tf~5 tf~6 tf A 7;
o 1 2*tf 3*tf~2 4*tf~3 5*tf A 4 6*tf~5 7*tf A6;
o 0 2 6*tf 12*tf~2 20*tf A3 30*tf~4 42*tf~5;
o 0 0 6 24*tf 60*tf A2 120*tfA3 210*tf A4};
phiO=[O;O;O;O;2*pi;O;O;0]; a=inv(A)*phiO; phic=a' dphic=[phic(2)
phic(3) phic(4) phic(S) phic(6) phic(7) phic(8)] ddphic=[phic(3}
phic(4) phic(S) phic(6) phic(7) phic(8) J
x=xc+r;y=yc; d=(xA2+y~2l1212~2)/(2*11*12);q2=acos(d);
ql=atan(y/x)atan( (12*sin(q2))/(11+l2*cos(q2)) ); xO=[ql;q2;0;0];
%xO=[ql+.0;q2+.1;0;0] %position uncertainty
T=0:0.002:tf; options = odeset('ReITol',le3);
[t,x]=ode23('pddiff' ,T,xO,options);
close all
%Output equations
ql=x(:,l); q2=x(:,2); qldot=x(:,3); q2dot=x(:,4);
% ternpdata(nurnl,l)=t;
% ternpdata(nurnl,2)=dqdot(1);ternpdata(nurnl,3)=dqdot(2);
% ternpdata(nurnl,4)=u(1);tempdata(numl,S)=u(2);
tt=tempdata(:,l) ;
trajref; %get desired trajectory
xx=11*cos(ql)+12*cos(ql+q2); yy=11*sin(ql)+12*sin(ql+q2);
%%%% Plot the payload and angular error
%set (fig, / Position' , [0 60 800 660 J ) ;
subpIot(2,l,l); plot([O;tf/2;tf/2+.001;tfl,[O;0;mp;mp]); axis([O 4
o mp]); grid on; titIe(/Payload (PO control)/); ylabel(/Payload Mp
(N) , ); subplot (2, 1/ 2) ; plot (t, qlqd ( : / 1) , ,  / , t, q2 qd ( : ,2) , , ' ) ;
grid on; xlabel (' t (sec)'); ylabel (/ ~l<L..l_d, ~2a,2_d (rad)');
titIe('Angular Error(PO control)/); legend('Errl' ,'Err2' ,0); print
122
c:\zhu\robotics\payload~errpd.eps dps
B3 Adptvfpayloadl.m
global 11 global 12 global pI p2 p3 global tf global r xc yc
global gama fv tau global phic dphic ddphic global num1 global
tempdata global fl f2 gl g2 fg taufg global ppl pp2 pp3
£1=.05; £2=.05; gl=.OI; g2=.01; fg=[£1;£2;gl;g2];
%fg=[O;O;O;O];
taufg=1.0*eye(4);
11=.38; 12=.24; pl=3.4;p2=.4;p3=.3;
%mp=50;ppl=pl;pp2=p2;pp3=p3; % payload =50(N)
tf=4; r=.I;xc=0.48;yc=0; gama=eye(2) *100; fv=200*eye(2);
tau=1.0*eye(3); numl=O;