بغزةميةســمعة الجا اThe Islamic University of Gaza عليات الســـاة الدرا عمادDeanship of Post Graduated Studies يـة الهندســـــــــــــــة لُ كFaculty of Engineering قسملكهربائية الهندسة اElectrical Engineering Department Modeling and High Precision Motion Control of 3 DOF Parallel Delta Robot Manipulator By Eng. Hamdallah A. H. Alashqar 354/2007 Advisor Prof. Dr. Mohammed T. Hussein A Thesis Submitted in Partial Fulfillment of the Requirements for the Degree of Master in Electrical Engineering Gaza-Palestine 1434-2013
98
Embed
Modeling and High Precision Motion Control of 3 DOF ... · Modeling and High Precision Motion Control of 3 DOF Parallel Delta Robot ... 2 Kinematics and Dynamics ... two types of
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
The Islamic University of Gaza الجامعة اإلســالمية بغزة
Deanship of Post Graduated Studies عمادة الدراســـات العليا
Faculty of Engineering ُكليـة الهندســـــــــــــــة
الهندسة الكهربائيةقسم Electrical Engineering Department
Modeling and High Precision Motion Control
of 3 DOF Parallel Delta Robot Manipulator
By
Eng. Hamdallah A. H. Alashqar
354/2007
Advisor
Prof. Dr. Mohammed T. Hussein
A Thesis Submitted in Partial Fulfillment of the Requirements for the Degree of Master in Electrical Engineering
Gaza-Palestine
1434-2013
2
Thesis approval
3
بِّ ِزْدنِي ِعْلًما َوقُل رَّ[111]طه:
i
Dedication
To the teacher of the world, leader of the nation and mercy of Allah to mankind,
Prophet Muhammad peace be upon him
To my lovely parents who are honor by this moment
To my Wife and my sweet daughter Lian
ii
Acknowledgements First and forever, all praise and thanks for Allah, who gave me the strength,
and patience to carry out this work in this good manner. I would like to
deeply thank my supervisor Prof. Dr. Mohammed Hussein for his assistance,
guidance, support, patience, and encouragement. I would like to deeply thank
my discussion committee members Dr. Hatem Elaydi and Dr. Iyad Abu-
hadrous for their assistance and encouragement. I would like to thank the
Islamic University of Gaza providing me a good academic environment. In
addition, I would like to acknowledge the Academic staff of Electrical
Engineering, who supports me to carry out this work. Finally, great thanks
go to my beloved family for their endless praying and continuous support.
iii
ABSTRACT
This Master thesis describes the CAD- Modeling of the Parallel DELTA robot,
designed by Autodesk Inventor® software program. DELTA Robot is a Multi-
Input Multi Output Nonlinear System (MIMO), so, PID controller and Model
Predictive Controller (MPC) are implemented to improve the performance of
Robot .but due to the variations in the dynamic models of each system, it is
nearly impossible to conclusively determine the most appropriate controller
to design. Therefore, this thesis compares the simulation results of two
controllers, namely the PID and MPC respectively; on a 3 DOF Parallel
DELTA robot in order to determine which controller would yield the best
control performance.
By comparing the simulation results for the joint angles error and the end
effector trajectory error plots for the PID and MPC controllers, MPC
controller gave the best results than PID controller. Then, a great
contribution added at the response of DELTA robot. Because of Robot arms
are highly geared; this reason let the robot to be more robust. MPC controller
held the Potential to be the most likely candidate controllers to implement on
the physical structure of the 3-DOF Parallel DELTA robot. But PID
controller is easier in software implementation inside embedded systems as
microcontrollers.
iv
ملخص:
التمثيل الرياضي و التحكم عالي الدقة بالروبوت المتوازي "دلتا" ثالثي األبعاد
للروبوت عن طريق التصميم ثالثي األبعاد باستخدام برنامجاقترحت في هذه الرسالة التمثيل الرياضي
(Autodesk Inventor)المخارج غير خطي , لذلك ومتعدد المداخل دعد.الروبوت عبارة عن نظام مت
( لتحسين مستوي الدقة واألداء للروبوت. كل نظام له MBC( أو ) PIDفهو بحاجة الي نظام تحكم )
ذلك يصعب تحديد نوع المتحكم لكل نظام بدقة . لذلك هذه الرسالة تقارن نتائج نموذج ديناميكي خاص به , ل
( علي الروبوت المتوازي دلتا ثالثي األبعاد واختيار MBC( و )PIDتطبيق نوعين من المتحكمات )
المتحكم الذي يضمن للروبوت األداء األفضل.
( , اثبتت النتائج MBC( ونظام التحكم )PIDل ومقارنة النتائج المتوقعة من كال نظامي التحكم )يبعد تمث
أعطي (MBC)لكن المتحكم حسينا ً كبيرا ً علي حركة الروبوتحكمين أضافا تتالماإلفتراضية بأن كال
( أفضل في التمثيل والتنفيذ PIDلكن بالرغم من هذا فإن نظام التحكم ) . (PID)نتائج أفضل من المتحكم
لسهولة برمجته علي انضمة التحكم الدقيق ) المايكروكنترولر(.عمليا علي الروبوت دلتا وذلك
v
Table of Contents:
Dedication I
Acknowledgments II
Abstract III
List of Figures VIII
List of Tables X
List of Abbreviations XI
1 Introduction …………………………………………………..
1
1.1 Backgrounds………………………………………………………….. 1
1.1 Motivation…………………………………………………………..... 4
1.3 Objectives and Methodology ………………………………………. 4
1.4 Problem Statement………………………………………………….. 5
1.5 Thesis Contribution…………………………………………………. 6
1.6 Literature Review……………………………………………………. 6
1.7 Thesis Overview……………………………………………………... 7
2 Kinematics and Dynamics…………………………………
8
2.1 DELTA Type Parallel Robot……………………………………….. 8
2.2 Inverse Kinematics………………………………………………….. 9
2.3 Forward Kinematics…………………………………………………. 15
2.4 Velocity Kinematics…………………………………………………. 22
2.5 Forward and Inverse Singularity analysis 26
2.6 Dynamic Equations………………………………………………….. 26
2.6.1 Virtual Work Dynamics…………………………………….. 28
2.6.2 No-Rigid Body Effects………………………………………. 31
2.7 Actuator Dynamics…………………………………………………... 33
3 Controller Design
35
3.1 Controller Techniques ……………………………………………. 35
3.2 Open and Closed-Loop Control………………………………….. 35
vi
3.2.1 Robot Control Algorithms………………………………………... 37
3.3 Computed Torque Control………………………………………... 39
3.4 PID Outer-Loop Designs………………………………………….. 40
3.5 PD-Plus-Gravity Controller………………………………………. 41
3.6 Optimal PD Controller Design…………………………………… 42
3.7 Model Predictive Control (MPC)………………………............... 45
4 Path Planning
48
4.1 Introduction………………………………………………….... 48
4.2 Cubic Polynomials……………………………………………. 49
4.2.1 Cubic Polynomials for a path with via points…………. 51
5.4 Dynamic Model of Delta Robot………………………………….. 55
5.5 Model Linearization………………………………………………. 57 5.5.1 Trimming and Linearizing Through Inverse Dynamics…... 59 5.5.2 Linearizing at an Operating Point……………………………. 59
After substitution the values 1 =0, 2 = 120, and 3 =-120 in Eq. 13, we get the
three sphere equations,
2 2 2 2
1 1
2 2 2 2
2 2 2
2 2 2 2
3 3 3
( [( ) cos( )]) ( ) ( sin( ))
1 3( [( ) cos( )]) ( [( ) cos( )]) ( sin( ))
2 2
1 3( [( ) cos( )]) ( [( ) cos( )]) ( sin( ))
2 2
a a b
a a a b
a a a b
x f e l y z l l
x f e l y f e l z l l
x f e l y f e l z l l
(2.18)
Rearrange Eq. 2.18 we obtain,
2 2 2 2
11 12 13
2 2 2 2
21 22 23
2 2 2 2
31 32 33
( ) ( ) (z )
( ) ( ) ( )
( ) ( ) ( )
b
b
b
x k y k k l
x k y k z k l
x k y k z k l
(2.19)
Where,
11 1
12
13 1
( ) cos( )
0
sin( )
a
a
k f e l
k
k l
21 2
22 2
23 2
1[( ) cos( )]
2
3[( ) cos( )]
2
sin( )
a
a
a
k f e l
k f e l
k l
31 3
32 3
33 3
1[( ) cos( )]
2
3[( ) cos( )]
2
sin( )
a
a
a
k f e l
k f e l
k l
After expanding Eq. 2.19 we obtain,
19
2 2 2 2 2 2 2
1 2 3 1 2 32 2 y 2 z ( ) , i=1,2,3i i i b i i ix y z k x k k l k k k (2.20)
Subtract Eq.2.20 with i=2 from Eq. 2.20 with i=1, we obtain,
2 2 2 2 2 2
11 21 12 22 13 23 21 22 23 11 12 132( ) 2( ) y 2( )z ( ) ( )k k x k k k k k k k k k k (2.21)
Subtract Eq.2.20 with i=3 from Eq. 2.20 with i=1, we obtain,
2 2 2 2 2 2
11 31 12 32 13 33 31 32 33 11 12 132( ) 2( ) y 2( )z ( ) ( )k k x k k k k k k k k k k (2.22)
Simplifying Eq.2.21 and Eq.2.22 we obtain,
1 1 1 1
2 2 2 2
a x b y c z d
a x b y c z d
(2.23)
Where,
1 11 21
1 12 22
1 13 23
2( )
2( )
2( )
a k k
b k k
c k k
2 11 31
2 12 32
2 13 33
2( )
2( )
2( )
a k k
b k k
c k k
And, 2 2 2 2 2 2
1 21 22 23 11 12 13
2 2 2 2 2 2
2 31 32 33 11 12 13
( ) ( )
( ) ( )
d k k k k k k
d k k k k k k
Arranging Eq.2.23 in a matrix form, we obtain,
20
1 1 1 1
2 2 2 2
a b d c zx
a b d c zy
(2.24)
Define 1 2 2 1a b a b , then for case 0 ,
1 1 2 2 2 1
2 1 1 2 1 2 2 1
2 2 2 1 1 1
1 2 2 1 2 1 1 2
( ) ( )
( ) ( ) z
( ) ( )
( ) ( ) z
x d c z b d c z b
b d b d b c b c
y d c z a d c z a
a d a d a c a c
2 1 1 2 1 2 2 1
1 2 2 1 2 1 1 2
b d b d b c b cxx z
a d a d a c a cyy z
(2.25)
Consider,
2 1 1 2 1 2 2 11 2
1 2 2 1 2 1 1 2
,
, x y
b d b d b c b cf f
b c b c a c a cf f
Then,
1
2
x
y
x f f z
y f f z
(2.26)
Substituting Eq. 2.26 in Eq.2.19 for i=3; we obtain,
2 2 2 2 2 2 2
1 31 2 32 33 11 22 33(1 )z 2([ ] [ ] ) 0x y x x y y bf f f f f k f f f k k z f f k l (2.27)
21
Let, 2 2
1 31 2 32 33
2 2 2 2
11 22 33
(1 )
2([ ] [ ] )
x y
x x y y
b
A f f
B f f f k f f f k k
C f f k l
Where,
11 1 31
22 2 32
f f k
f f k
The solution of Equation 2 0Az Bz C is well known as
2 4
2
B B ACz
A
(2.28)
From Eq.2.28, we can Evaluates Eq.2.26.
Mathematically neither forward nor inverse kinematics gives single solution.
Forward kinematics usually has two solutions, because the passive joint
angles formed between upper arm and lower arm are not determined by
kinematic equations. Then the solution that is within the robots work area
must be chosen. With the base frame {O} in this case, it will lead to the
solution with negative z coordinate.
The output solution has Four cases are possible:
1) Generic solution. The two solutions are realized at the intersection of a
circle and a sphere.
2) Singular solution. Once sphere is tangent to the circle of intersection of
the other two spheres, hence there is only one solution possible.
3) Singular solution. The center of any two spheres coincides, resulting in
an infinite number of solutions. This is an unlikely configuration for
most practical embodiments of the manipulator, except for the situation
when θ1 = θ2 = θ3 = π/2.
4) No solution. The three spheres do not intersect at a common point.
22
2.4 Velocity Kinematics
The most relevant loop should be picked up for the intended Jacobian
analysis. Let θ be the vector made up of actuated joint variables and P is the
position vector of the moving platform. Then
Figure 2.8: (a) Projection of link i on xi zi plane, (b) end on view
11
1 12
13
,i
x
y
z
θ P (2.29)
The Jacobian matrix will be derived by differentiating the appropriate loop
closure equation and rearranging the result in the following form
23
11
11
11
x x
y y
z z
P v
P v
P v
θ PJ J (2.30)
where vx, vy, and vz are the x, y, and z components of the velocity of the point
P on the moving platform in the xyz frame. In order to arrive at the above
form of the equation, we look at the loop OAiBiCiP. The corresponding
closure equation in the xiyizi frame is
i i i i i iOP + PC = OA + A B + B C (2.31)
In the matrix form we can write it as
1 3 2 1
3
1 3 2 1
cos cos cos sin cos( )
sin cos 0 0 0 sin
0 0 sin sin cos( )
x i y i i i i i
x i y i a b i
z i i i i
P P f e
P P l l
P
(2.32)
Time differentiation of this equation leads to the desired Jacobian equation.
The loop closure equation Eq.2.31 can be re-written as
i i(P +e) = f +a +b (2.33)
Where ia and
ib represents vectors i iA B and
i iB C respectively.
Differentiating Eq.2.33 with respect to time and using the fact that f is a
vector characterizing the fixed platform, and e is a vector characterizing the
moving platform
i iP = v = a + b (2.34)
The linear velocities on the right hand side of Eq.2.34 can be readily
converted into the angular velocities by using the well-known identities.
24
Thus
ai i bi iv = w ×a + w ×b (2.35)
aiw andbi
w is the angular velocity of the link i. To eliminatebi
w , it is
necessary to dot-multiply both sides of Eq. 2.35 and bi. Therefore
i ai i ib .v = w . (a ×b ) (2.36)
Rewriting the vectors of Eq.2.36 in the xiyizi coordinate frame leads to
1 3 2 1
3
1 3 2 1
1
cos sin cos( )
0 , sin
sin sin cos( )
0 cos cos
, sin cos
0
i i i i
i a i b i
i i i i
x i y i
i i x i y i
z
a l b l
v v
w v v v
v
Substituting the values of ai, bi,vi and v in Eq.2.36 leads to
2 3 1sin sinix x iy y iz z a i i ij v j v j v l (2.37)
Where,
1 2 3 3
1 2 3 3
1 2 3
cos( )sin cos cos sin
cos( )sin sin cos cos
sin( )sin
ix i i i i i i
iy i i i i i i
iz i i i
j
j
j
Expanding Eq.2.37 for i = 1, 2 and 3 yields three scalar equations which can
be assembled into a matrix form as
x qj v = j q (2.38)
25
Where,
1 1 1
2 2 2
3 3 3
21 31
22 32
23 33
sin sin 0 0
0 sin sin 0
0 0 sin sin
x y z
x y z
x y z
a
j j j
j j j
j j j
l
x
q
j
j
11 12 31
T
q
After algebraic manipulations, it is possible to write
v = Jq (2.39)
Where,
1 2 3
1 2 3
1 2 3
x y z
x y z
x y z
-1
x qJ = j j (2.40)
26
2.5 Forward and Inverse Singularity analysis
From Eq.2.38 it can be observed that singularity occurs:
1. when det(Jq) = 0. This means that either 2i = 0 or , 3i =0 or for i=1,2,3.
2. when det(Jx) = 0. This means that 1 2i i =0 or or 3i =0 or for i=1,2,3.
3. when det(Jq)=0 and det(Jx) =0. This situation occurs when 3i =0 or for
i=1,2 and 3.
In summary, singularity of the parallel manipulator occurs:
1. When all three pairs of the follower rods are parallel. Therefore, the
moving platform has three degrees of freedom and moves along a
spherical surface and rotates about the axis perpendicular to the
moving platform
2. When two pairs of the follower rods are parallel. The moving platform
has one degree of freedom; i.e. the moving platform moves in one
direction only.
3. When two pairs of the follower rods are in the same plane or two
parallel planes. The moving platform has one degree of freedom; i.e. the
moving platform rotates about the horizontal axis only.
2.6 Dynamic Equations
Dynamics is the science of motion. It describes why and how a motion occurs
when forces and moments are applied on massive bodies. The motion can be
considered as evolution of the position, orientation, and their time
derivatives. In robotics, the dynamic equation of motion for manipulators is
utilized to set up the fundamental equations for control. The links and arms
in a robotic system are modeled as rigid bodies.
Therefore, the dynamic properties of the rigid body take a central place in
robot dynamics. Since the arms of a robot may rotate or translate with
27
respect to each other, translational and rotational equations of motion must
be developed and described in body-attached coordinate frames B1, B2, B3 …
or in the global reference frame G.
There are basically two problems in robot dynamics.
Problem1. We want the links of a robot to move in a specified manner. What
forces and moments are required to achieve the motion?
The first Problem is called direct dynamics and is easier to solve when the
equations of motion are in hand because it needs differentiating of kinematics
equations. The first problem includes robots statics because the specified
motion can be the rest of a robot. In this condition, the problem reduces
finding forces such that no motion takes place when they act. However, there
are many meaningful problems of the first type that involve robot motion
rather than rest. An important example is that of finding the required forces
that must act on a robot such that its end-effector moves on a given path and
with a prescribed time history from the start configuration to the final
configuration.
Problem2. The applied forces and moments on a robot are completely
specified. How will the robot move?
The second problem is called inverse dynamics and is more difficult to solve
since it needs integration of equations of motion. However, the variety of the
applied problems of the second type is interesting. Problem 2 is essentially a
prediction since we wish to find the robot motion for all future times when
the initial state of each link is given.
In this section, we will perform the inverse dynamic modeling of the parallel
manipulator based upon the principle of virtual work. The inverse dynamics
problem is to find the actuator torques and/or forces required to generate a
desired trajectory of the manipulator.[theory of applied robotics boo]
It is often convenient to express the dynamic equations of a manipulator in a
single equation that hides some of the details, but shows some of the
structure of the equations. The state-space equation When the Newton—
Euler equations are evaluated symbolically for any manipulator, they yield a
dynamic equation that can be written in the form
( ) ( , ) ( ) τ M V G (2.41)
28
where ( )M is n x n mass matrix of the manipulator, ( , ) V is a n x 1
vector of centrifugal and Coriolis terms, and ( )G is an n x 1 vector of gravity
terms. We use the term state-space equation because the term ( , )V has both
position and velocity dependence. Each element of ( )M and ( )G is a
complex function that depends on θ, the position of all the joints of the
manipulator. Each element of ( , ) V is a complex function of both and .
We may separate the various types of terms appearing in the dynamic
equations and form the mass matrix of the manipulator, the centrifugal and
Coriolis vector, and the gravity vector [1].
2.6.1 Virtual Work Dynamics
In this section, we will perform the inverse dynamic modeling of the parallel
manipulator based upon the principle of virtual work. The inverse dynamics
problem is to find the actuator torques and/or forces required to generate a
desired trajectory of the manipulator [9].
Without losing generality of model, we can simplify the dynamic problem by
the following hypotheses:
The connecting rods of lower links can be built with light materials such as
the aluminum alloy, so
The lower links rotational inertias are neglected.
the mass of each lower links, is divided evenly and concentrated at
The two endpoints of the parallelogram.
Also it is supposed that:
• The friction forces in joints are neglected.
• No external forces suffered.
We consider that 1 2 3[ , , ] and 1 2 3[ , , ] are the vector of
actuator torques and vector of corresponding virtual angular displacements.
Furthermore, [ , , ]p x y z represents the virtual linear displacements
vector of the mobile platform. We can derive the following equations by
applying the virtual work principle.
29
0T T T T T
Ga Gp a pM F p M F p (2.42)
Where,
1 2 3
1( ).g .I.[cos( ) cos( ) cos( )]2
T
Ga a b aM m m l (2.43)
is the upper links gravity torques vector ma and mb are mass of upper link
and each connecting rod of lower link, respectively. Here g denotes the
gravity acceleration, and I represent the 3x3 identity matrix.
[0 0 ( 3 )g)]T
Gp tcp bF m m (2.44)
Denotes the mobile platform gravity force vector, and mtcp is mass of the
mobile platform.
1 2 3[ ]T
a aaM II (2.45)
Where,
2 21( ).I3
a a a b aI m l m l
Represents the upper links inertia torques vector and denotes the upper links
inertial matrix with respect to the fixed frame O{x, y, z}, and,
( 3 ).I.[x y z]T
P P tcp bF M P m m (2.46)
Denote the mobile platform inertial forces vector. Eq.2.39 in section 2.4 can
be rewritten to,
P = Jθ (2.47)
30
Consequently,
P J (2.48)
Substituting Eq. 2.48 into Eq. 2.42 results,
( ) 0T T T T T
Ga Gp a pM F J M F J (2.49)
Eq. 2.49 holds for any virtual displacements , so we have
T T
a p Ga GpM J F M J F (2.50)
Substitute Eqs.2.44 and 2.45 into Eq. 2.50, allows the generation of
T T
a p Ga GpI J M P M J F (2.51)
Differentiating Eq. 2.47 with respect to time, yields
P J J (2.52)
Substituting Eq. 2.52 into Eq. 2.51, we can derive that
( ) ( , ) ( )M V G
The previous equation described in Eq. 2.41 represents the dynamic model of
parallel manipulator in joint space. Here, 3R is the controlled variables,
and
( ) T
a pM I J M J (2.53)
Denotes a symmetric positive definite inertial matrix, that3 3( ) xM R .
31
( , ) T
pV J M J (2.54)
Where 3 3( , ) xV R is the centrifugal and Coriolis forces matrix, and
( ) M T
Ga GpG J F (2.55)
Represents the vector of gravity forces, and3( )G R .
2.6.2 Non-Rigid Body Effects
It is important to realize that the dynamic equations we have derived do not
encompass all the effects acting on a manipulator. They include only those
forces which arise from rigid body mechanics. The most important source of
forces that are not included is friction. All mechanisms are, of course, affected
by frictional forces. In present-day manipulators, in which significant gearing
is typical, the forces due to friction can actually be quite large - perhaps
equaling 25% of the torque required to move the manipulator in typical
situations. In order to make dynamic equations reflect the reality of the
physical device, it is important to model (at least approximately) these forces
of friction. A very simple model for friction is viscous friction, in which the
torque due to friction is proportional to the velocity of joint motion. Thus, we
have
friction v (2.56)
where v is a viscous-friction constant. Another possible simple model for
friction, Coulomb friction, is sometimes used. Coulomb friction is constant
except for a sign dependence on the joint velocity and is given by
sgn( )friction c (2.57)
where c is a Coulomb-friction constant. The value of c is often taken at one
value when 0 the static coefficient, but at a lower value, the dynamic
32
coefficient, when 0 , whether a joint of a particular manipulator exhibits
viscous or Coulomb friction is a complicated issue of lubrication and other
effects. A reasonable model is to include both, because both effects are likely:
sgn( )friction v c (2.58)
It turns out that, in many manipulator joints, friction also displays a
dependence on the joint position. A major cause of this effect might be gears
that are not perfectly round-their eccentricity would cause friction to change
according to joint position. So a fairly complex friction model would have the
form
( , )friction f (2.59)
These friction models are then added to the other dynamic terms derived
from the rigid-body model, yielding the more complete model
( ) ( , ) ( ) ( , )M V G F (2.60)
There are also other effects, which are neglected in this model. For example,
the assumption of rigid body links means that we have failed to include
bending effects (which give rise to resonances) in our equations of motion.
However, these effects are extremely difficult to model and are beyond the
scope of this thesis [1].
33
2.7 Actuator Dynamics
The leg system is basically composed of dc motor, precision revolute bearing
and coupling elements. Dc motor model is given below
Figure 2.9: DC motor model
The symbols represent the following variables here m is the motor position
(radian), m is the produced torque by the motor (Nm), 1 is the load torque, av
is the armature voltage (V), La is the armature inductance (H), Ra is the
armature resistance (Ω), Em is the reverse EMF (V), Ia is the armature current
(A), Kb is the reverse EMF constant, Km is the torque constant [10].
2
1 2
aa a a a m
mm b
m m a
mm m
diL R i V E
dt
dE K
dt
K i
dj
dt
(2.53)
On the assumption of a rigid transmission and with no backlash the
relationship between the input forces (velocities) and the output forces
(velocities) are purely proportional. This gives,
m r lK (2.54)
34
Where, constant Kr is a parameter which describes the gear reduction ratio. l
is the load torque at the robot axis and m is the torque produced by the
actuator at the shaft axis. In view of Eq. 2.54 one can write
lm
rK
(2.55)
To simulate the motion of a manipulator, we must make use of a model of the
dynamics such as the one we have just developed. Given the dynamics
written in closed form as in (2.52), simulation requires solving the dynamic
equation for acceleration:
1( )[ ( , ) ( ) ( , )]M V G F (2.56)
We can then apply any of several known numerical integration techniques to
integrate the acceleration to compute future positions and velocities. Given
initial conditions on the motion of the manipulator, usually in the form
(0)
(0) 0
(2.57)
35
Chapter 3
Controller Design
3.1 Controller Techniques
Using inverse kinematics, we can calculate the joint kinematics for a desired
geometric path of the end-effector of a robot. Substitution of the joint
kinematics in equations of motion provides the actuator commands. Applying
the commands will move the end-effector of the robot on the desired path
ideally. However, because of perturbations and non-modeled phenomena, the
robot will not follow the desired path. The techniques that minimize or
remove the difference are called the control techniques [11].
3.2 Open and Closed-Loop Control
A robot is a mechanism with an actuator at each joint i to apply a force or
torque to derive the link ( i ). The robot is instrumented with position,
velocity, and possibly acceleration sensors to measure the joint variables’
kinematics. The measured values are usually kinematics information of the
frame iB , attached to the link i . Relative to the frame 1iB or 0B . To cause each
joint of the robot to follow a desired motion, we must provide the required
torque command. Assume that the desired path of joint variables, ( )d q tq are
given as functions of time. Then, the required torques that causes the robot to
follow the desired motion is calculated by the equations of motion and is
equal to
,( ) ) )( (d d d d dc
q q q q q Q D H G (3.1)
Where the subscripts d and c stands for desired and controlled, respectively. in
an ideal world, the variables can be measured exactly and the robot can
36
perfectly work based on the equations of motion (3.1). Then, the actuators’
control command Qc can cause the desired path qd to happen. This is an open-
loop control algorithm, that the control commands are calculated based on a
known desired path and the equations of motion. Then, the control
commands are fed to the system to generate the desired path. Therefore, in
an open-loop control algorithm, we expect the robot to follow the designed
path, however, there is no mechanism to compensate any possible error.
Now assume that we are watching the robot during its motion by measuring
the joints’ kinematics. At any instant there can be a difference between the
actual joint variables and the desired values. The difference is called error
and is measured by
d
d
e q q
e q q
(3.2)
Let’s define a control law and calculate a new control command vector by
= c d p
K K Q Q e e (3.3)
where kP and kd are constant control gains. The control law compares the
actual joint variables ( , )q q with the desired values ( , )d d
q q , and generates a
command proportionally. Applying the new control command changes the
dynamic equations of the robot to produce the actual joint variables q .
,( ) ( ) ( )d d d d dc d pq q q q qK K H GQ e e D (3.4)
37
Figure 3.1 Illustration of feedback control algorithm
Fig. 3.1 illustrates the idea of this control method in a block diagram. This is
a closed-loop control algorithm, in which the control commands are calculated
based on the difference between actual and desired variables. Reading the
actual variables and comparing with the desired values is called feedback, and
because of that, the closed-loop control algorithm is also called a feedback control algorithm.
The controller provides a signal proportional to the error and its time rate.
This signal is added to the predicted command Qc to compensate the error.
The principle of feedback control can be expressed as: Increase the control command when the actual variable is smaller than the desired value and decrease the control command when the actual variable is larger than the desired value.
3.2.1 Robot Control Algorithms
Robots are nonlinear dynamical systems, and there is no general method for
designing a nonlinear controller to be suitable for every robot in every
mission. However, there are a variety of alternative and complementary
methods, each best applicable to particular class of robots in a particular
mission. The most important control methods are as follows:
Feedback Linearization or Computed Torque Control Technique.
In feedback linearization technique, we define a control law to obtain a linear
differential equation for error command, and then use the linear control
38
design techniques. The feedback linearization technique can be applied to
robots successfully; however, it does not guarantee robustness according to
parameter uncertainty or disturbances. This technique is a model-based
control method, because the control law is designed based on a nominal
model of the robot.
Linear Control Technique
The simplest technique for controlling robots is to design a linear controller
based on the linearization of the equations of motion about an operating
point. The linearization technique locally determines the stability of the
robot. Proportional, integral, and derivative, or any combination of them, are
the most practical linear control techniques.
Adaptive Control Technique
Adaptive control is a technique for controlling uncertain or time-varying
robots. Adaptive control technique is more effective for low DOF robots.
Robust and Adaptive Control Technique.
In the robust control method, the controller is designed based on the nominal
model plus some uncertainty. Uncertainty can be in any parameter, such as
the load carrying by the end-effector. For example, we develop a control
technique to be effective for loads in a range of 1 - 10 kg.
Gain-Scheduling Control Technique.
Gain-scheduling is a technique that tries to apply the linear control
techniques to the nonlinear dynamics of robots. In gain-scheduling, we select
a number of control points to cover the range of robot operation. Then at each
control point, we make a linear time-varying approximation to the robot
dynamics and design a linear controller. The parameters of the controller are
then interpolated or scheduled between control points.
39
3.3 Computed Torque Control
Dynamics of a robot can be expressed in the form
,( ) ( ) ( )d
q q q q q H GQ D (3.5)
Where q is the vector of joint variables, and Q is the torques applied at joints,
And is d a disturbance .Assume a desired path in joint space is given by a
twice differentiable function 2( ) d t Cq q . Hence, the desired time history of
joints’ position, velocity, and acceleration are known [12].
We can re-write Eq. 3.5 to:
,( ) ( )d
q q q q Q D N (3.6)
If this equation includes motor actuator dynamics, then Q is an input voltage.
Define an output or tracking error as:
de q q (3.7)
And so,
d
d
e q q
e q q
(3.8)
Solving now for q in Eq.3.6 and substituting into Eq. 3.7 yields,
1 )(Nd de q D (3.9)
And the disturbance function
1
dw D
(3.10)
40
we may define a state x(t) by
ex
e
(3.11)
Write the tracking error dynamics as,
0 0 0
0 0
e
e
I eu w
e I I
(3.12)
It is driven by the control input u(t) and the disturbance w(t). Note that this
derivation is a special case of the general feedback linearization procedure.
The feedback linearizing transformation may be inverted to yield
( u) Nd
D q (3.13)
We call this the computed-torque control law. Substituting Eq. 3.13 into
Eq.3.5 yields
, )( )( ) ( u Nd d
q q q q q D DN (3.14)
or
1d
e u D (3.15)
3.4 PID Outer-Loop Designs
One way to select the auxiliary control signal u(t) is as the proportional-plus
derivative (PD) feedback,
v b iu k e k e k (3.16)
41
Then the overall robot arm input becomes
,( )( )( )v b i
q q q qk e k e k D N (3.17)
The closed loop error dynamics
v b ie k e k e k w (3.18)
The next diagram, represent the PD – Computed Torque controller
3.5 PD-Plus-Gravity Controller
A useful controller in the computed-torque family is the PD-plus-gravity
controller that results when D=I, N=G(q)-qd, with G(q) the gravity term of the
manipulator dynamics. Then, selecting PD feedback for u(t) yields,
( )c v b
k e k e G q (3.19)
∑ ∑
Robot
Figure 3.2. Block diagram of computed torque control.
∑
∑
42
When the arm is at rest, the only nonzero terms in the dynamics Eq.3.5 are
the gravity G (q), the disturbance d , and possibly the control torque .
The PD-gravity controller c , includes G (q), so that we should expect good
Performance for set-point tracking, that is, when a constant qd is given so
that qd = 0.
3.6 Optimal PD Controller Design The goal of implementing any type of controller is to observe the output
response it would generate based on the inputted conditions. In order to
achieve this, it is necessary to solve for the control input (u) of the system.
Each controller has a different method pertaining to how this equation is
obtained, but the initial steps to reach this point are all similar.
The end effector of the three degrees of freedom parallel robot will follow a
predefined trajectory; hence for tracking control it is appropriate to set the
error and change in error as:
1 1 1
2 2 2
3 3 3
4 1 1
5 2 2
6 3 3
,
,
,
,
,
,
d
d
d
d
d
d
x
x
x
x
x
x
Where: 1d , 2d and 3d are the desired angles; 1 , 2 and 3 are the actual
angles; 1d , 2d and 3d are the desired angular velocities, 1 , 2 and 3 are the
actual angular velocities; 1d , 2d and 3d the desired angular accelerations.
The following system is in lower triangular form, which can be produced by
differentiating 1 2 3 4 5 6, , , , x x x x x and x .
1 4x x
43
2 5x x
3 6x x (3.20)
4 1 1
1
5 2 2
6 3 3
( ) ( , ) ( )
d
d
d
x
x D u C G
x
(3.21)
One aspect that constantly appears when implementing the appropriate
controller is the feed forward term ud. This term represents the desired
control input required in the overall system operation. In theory, the actual
and desired control input should be identical, but due to system disturbances
and the force of gravity, this is known not to be the case. By adding ud into
the specified controller, improved control performance can be achieved. It is
defined as:
( ) ( , ) ( )d d d d d d du D C G (3.22)
That is, a Lyapunov function is necessary in order to achieve the desired
results. Let this function candidate be:
1 1 4
1 2 3 2 2 4 5 6 5
3 3 6
0 0
0.5 0 0 0.5 ( )
0 0
kp x x
V x x x kp x x x x D x
kp x x
(3.23)
It should be noted that KP1 , KP2 and KP3 represent the proportional gains of
motors one , two and three respectively. The derivative of V with respect to
time becomes:
44
1 4 4
1 2 3 2 5 4 5 6 5
3 6 6
4
4 5 6 5
6
0 0
0 0 0.5 ( )
0 0
+ ( )
kp x x
V x x x kp x x x x D x
kp x x
x
x x x D x
x
(3.24)
As previously stated, ( ) 2 ( , )D C is skew symmetric; hence:
4 4
4 5 6 5 4 5 6 5
6 6
0.5 ( ) ( , )
x x
x x x D x x x x C x
x x
(3.25)
Therefore, by substituting equations (3.3) and (3.25) into equation (3.24), it is
possible to achieve:
1 4 1 1 1
1 2 3 2 5 4 5 6 2 2 2
3 6 3 3 3
0 0
0 0 ( ) ( , )) ( )
0 0
d d
d d
d d
kp x u
V x x x kp x x x x D C G u
kp x u
(3.26)
With all the appropriate data defined, it is now possible to determine the
equation for the controller. The control effort must satisfy the condition of
convergence and it must ensure that the output response is stable. The
following controller was chosen to accomplish these requirements:
45
1 1 1 1 1 1
2 2 2 2 4 5 6 2 2
3 3 3 3 3 3
0 0 0 0
0 0 0 0 ( ) ( , ) ( )
0 0 0 0
d d
d d
d d
u kp x kd
u u kp x kd x x x D C G
u kp x kd
(3.27)
It should be noted that Kd1 , Kd2 and Kd3 represent the derivative gains of motors one,
two and three, respectively.
3.7 Model Predictive Control (MPC)
Model Predictive Control (MPC) is an optimal control strategy based on
numerical optimization. Future control inputs and future plant responses are
predicted using a system model and optimized at regular intervals with
respect to a performance index. From its origins as a computational
technique for improving control performance in applications within the
process and petrochemical industries, predictive control has become arguably
the most widespread advanced control methodology currently in use in
industry. MPC now has a sound theoretical basis and its stability, optimality,
and robustness properties are well understood.
The basic structure of MPC to implement is shown in Fig.3.3. A model is used
to predict the future plant outputs, based on past and current values and on
proposed future control actions.
Figure 3.3: Basic Structure of MPC Controller.
These actions are calculated by optimizer taking into account the cost
function as well as con- strains. The optimizer is another fundamental part of
+
-
46
the strategy as it provides the control action. The goal is to apply the linear
model predictive control to the input-output linearized system to account for
the constraints. Since the linear model predictive is more naturally
formulated in discrete time, the linear subsystem is discretized with a
sampling period T to yield,
(k 1) A (k) B (k)
y(k) H (k)
d d
d
V
(3.28)
The model consists of:
A model of the plant to be controlled, whose inputs are the manipulated
variables, the measured disturbances, and the unmeasured
disturbances
A model generating the unmeasured disturbances
Figure 3.4: Model Used For Optimization
The model of the plant is a linear time-invariant system described by the
equations
x(k 1) Ax(k) B u(k) B (k) B (k)
y(k) C (k) D (k) D (k)
u v d
x v d
v d
x v d
(3.29)
The unmeasured disturbance d(k) is modeled as the output of the linear time
invariant system:
47
x (k 1) Ax (k) Bn (k)
d(k) Cx ( ) (k)
d d d
d dk Dn
(3.30)
The system described by the above equations is driven by the random
Gaussian noise nd(k), having zero mean and unit covariance matrix.
48
Chapter 4
Path Planning
4.1 Introduction
In this chapter, we concern ourselves with methods of computing a trajectory
that describes the desired motion of a manipulator in multidimensional
space. Here, trajectory refers to a time history of position, velocity, and
acceleration for each degree of freedom.
This problem includes the human-interface problem of how we wish to specify
a trajectory or path through space. In order to make the description of
manipulator motion easy for a human user of a robot system, the user should
not be required to write down complicated functions of space and time to
specify the task. Rather, we must allow the capability of specifying
trajectories with simple descriptions of the desired motion, and let the system
figure out the details. For example, the user might want to be able to specify
nothing more than the desired goal position and orientation of the end-
effector and leave it to the system to decide on the exact shape of the path to
get there, the duration, the velocity profile, and other details.
We also are concerned with how trajectories are represented in the computer
after they have been planned. Finally, there is the problem of actually
computing the trajectory from the internal representation—or generating the
trajectory.
Generation occurs at run time; in the most general case, position, velocity,
and acceleration are computed. These trajectories are computed on digital
computers, so the trajectory points are computed at a certain rate, called the
path-update rate. In typical manipulator systems, this rate lies between 60
and 2000 Hz [1].
49
4.2 Cubic polynomials
Consider the problem of moving the tool from its initial position to a goal
position in a certain amount of time. Inverse kinematics allows the set of
joint angles that correspond to the goal position and orientation to be
calculated
Figure 4.1: Several possible path shapes for a single joint.
The initial position of the manipulator is also known in the form of a set of
joint angles. What is required is a function for each joint whose value at t0 is
the initial position of the joint and whose value at tf is the desired goal
position of that joint. As shown in Fig. 4.1, there are many smooth functions,( )t , that might be used to interpolate the joint value. Several possible path
shapes for a single joint. In making a single smooth motion, at least four
constraints on ( )t are evident. Two constraints on the function's value come
from the selection of initial and final values:
(0)
( )f ft
(4.1)
An additional two constraints are that the function be continuous in velocity,
which in this case means that the initial and final velocities are zero:
50
(0) 0
( ) 0ft
(4.2)
These four constraints can be satisfied by a polynomial of at least third
degree. These constraints uniquely specify a particular cubic. A cubic has the
form
2 3
1 2 3( )t a a t a t a t (4.3)
So, the joint velocity and acceleration along this path are clearly
2
1 2 3
2 3
( ) 2 3
( ) 2 6
t a a t a t
t a a t
(4.4)
Solving these equations for the ia we obtain
0 0
1
2 02
3 3
0
3( )
3(
f
f
f
a
a
at
at
(4.5)
Using Eq. 4.5, we can calculate the cubic polynomial that connects any initial
joint angle position with any desired final position. This solution is for the
case when the joint starts and finishes at zero velocity.
51
4.2.1 Cubic polynomials for a path with via points So far, we have considered motions described by a desired duration and a
final goal point. In general, we wish to allow paths to be specified that
include intermediate via points. If the manipulator is to come to rest at each
via point, then we can use the cubic solution of Eq. 4.3.
Figure 4.2: Via points with desired velocities at the points indicated by
tangents.
If desired velocities of the joints at the via points are known, then we can
construct cubic polynomials as before; now, however, the velocity constraints
at each end are not zero, but rather, some known velocity
(0)
( )f ft
(4.6)
Solving these equations for ia then we obtain
52
0 0
1 0
,
,
a
a
2 0 02
3 0 03 2
3 2 1( ) ,
3 1( ) ( )
f f
f f f
f f
f f
at t t
at t
(4.7)
53
Chapter 5
Simulations and Results
5.1 Introduction
Each controller discussed in this chapter will contain the simulation results
for: the error between the desired and actual actuated joint angles, the
location of the desired and actual end effector trajectory in Cartesian space
along with their respective positional output error and the overall system
torque required to achieve the actual results. A preliminary conclusion will
then be drawn based on the pros and cons of each control technique.
5.2 Modeling Multi-body Systems
SimMechanics tool enables you to create libraries of components that can be
reused in many different designs. You define bodies in terms of their mass,
inertia, and connection points. To create complex shapes, you assemble sets
of simple geometries, such as spheres, cylinders, and extrusions defined in
MATLAB, and SimMechanics calculates the resulting mass and inertia
automatically. The diagram that defines the body clearly indicates all
connections to the body, making it easy to see your system’s topology.
Parameters such as length and mass can be calculated using MATLAB
scripts and assigned using MATLAB variables.
You connect bodies using joints and constraints. These define the degrees
of freedom permitted between the bodies in your system, which dictate how
your system can move. You can define and connect actuators to these joints to
enable your system to move. Actuating these joints with electrical, hydraulic,
pneumatic, or other physical systems modeled using Simscape tool enables
you to model your entire multi-domain physical system within the Simulink
environment [13].
You can import a CAD assembly into SimMechanics using SimMechanics
Link. The mass and inertia of each part in the assembly are imported as rigid