Rafael Kelly Centro de Investigaci on Cient ca y de Educaci on Superior de Ensenada Mexico V ctor Santib an ez Instituto Tecnol ogico de la Laguna Mexico Antonio Lor a Centre National de la Recherche Scientique France
Springer Berlin Heidelberg New York Hong Kong London Milan Paris Tokyo
Part I PRELIMINARIES
Introduction
Introduction
Robots occupy a privileged place in the modernization of numerous industrial sectors. The word robot nds its origins in robota which means work in Slavic languages. Was introduced by the Czech science ction writer Karel Capek. The term robot is nowadays used to denote autonomous machines.
Introduction
These machines may be roughly classied as follows: Robot manipulators Ground robots Wheeled robots Legged robots Mobile robots Submarine robots Aerial robots. This book is exclusively devoted to robot manipulators.
Introduction
Robotics Term coined by the science ction writer Isaac Asimov Science devoted to the study of robots Incorporates a variety of elds: Electric engineering, mechanical engineering, industrial engineering, computer science and applied mathematics Automatic control of robot manipulators (spine of robotics).
Introduction
A manipulating industrial robot is an automatically controlled, reprogrammable, multipurpose manipulator programmable in three or more axes, which may be either xed in place or mobile for use in industrial automation applications.
Introduction
The number of joints of a manipulator determines as well, its number of degrees of freedom (DOF ) typically 6 DOF . 3 determine the position of the end of the last link in the Cartesian space 3 more specify its orientation.
q1
q2
q3
Introduction
In the present textbook: A robot manipulator or simply manipulator is a mechanical articulated arm that is constituted of links interconnected through hinges or joints that allow a relative movement between two consecutive links.
Introduction
The joint positions of the robot are collected in the vector q , i.e., q1 q2 . q := . . qn := The joint velocities are: q
d dt q
Robot control consists in studying how to make a robot manipulator perform a task. Control design may be divided roughly in the following steps: Familiarization with the physical system under consideration, Modeling. Control specications.
Figure 2: Freely moving robot. For this robot, the outputs y , are the positions q and joint velocities q or the position and orientation of the end eector.
R. Kelly, V. Santiba nez and A. Loria 10
In this case, the output y may include the torques and forces f exerted by the end tool over its environment.
11
Image
Camera
Figure 4: Robotic system: xed camera. In this system, the output y may correspond to the coordinates associated to each of the marks with reference to a screen on a monitor.
12
Image
Camera
Figure 5: Robotic system: camera in hand. Output y may correspond to the coordinates of the screen.
13
q

ROBOT q
Figure 6: Inputoutput representation of a robot. The input variables, are basically the torques and forces , The outputs are the joint positions and velocities: , f) = y = y (q , q q q
14
The systems mathematical model is obtained typically via one of the two following techniques. Analytic. Physics laws of the systems motion. Experimental. Experimental data collected from the system itself.
15
Some interesting topics related with modelling are: Robustness. Faculty of a control system to cope with errors due to neglected dynamics. Parametric identication. The objective is to obtain the numeric values of dierent physical parameters. The dynamic model of robot manipulators is derived in the analytic form using basically the laws of mechanics. an n DOF system (multivariable nonlinear system).
R. Kelly, V. Santiba nez and A. Loria 16
Control specications
Denition of control objectives: Stability Regulation Trajectory tracking (motion control) Optimization.
17
Stability. Consists in the property of a system by which it goes on working at certain regime or closely to it for ever. Lyapunov stability theory. inputoutput stability theory. In the case when the output y corresponds to the joint position q and . velocity q Regulation Position control in joint coordinates Trajectory tracking Tracking control in joint coordinates
18
Pointtopoint. Determines a series of points in the manipulators workspace, by which the end eector is required to pass.
(Continuous) trajectory. The control problem consists in making the endeector follow a trajectory as closely as possible.
Figure 8: Trajectory motion specication. Setpoint control problem. The specied trajectory is simply a point in the workspace.
R. Kelly, V. Santiba nez and A. Loria 20
Robot navigation problem consists in solving, in one single step, the following subproblems. Path planning. Determines a curve in the state space of the desired posture Trajectory generation. Parameterizes in time above curve Control design. Solves the control problem
21
Basic notation Throughout the text we employ the following symbols. for all there exists belong(s) to = implies is equivalent to or if and only if tends to := and =: is dened as and equals by denition to .
22
Vectors IRn denotes the real Euclidean space of dimension n Vector x of dimension n x1 x2 = [x1 x2 xn]T x= . . xn Denoted by bold small letters, either Latin or Greek.
R. Kelly, V. Santiba nez and A. Loria 23
x :=
i=1
x2 i
= xTx
Matrices IRnm denotes the set of real matrices A of dimension n m a11 a12 a1m a21 a22 a2m . A = {aij } = . . . . .. . . . an1 an2 anm
24
Eigenvalues
For each square matrix A IRnn there exist n eigenvalues (in general, complex numbers) denoted by 1{A}, 2{A}, , n{A}. satisfy: det [i{A}I A] = 0, for i = 1, 2, , n
For the case of a symmetric matrix A = AT IRnn, 1{A}, 2{A}, , n{A} IR. theorem of RayleighRitz establishes that for all x IRn Max{A} x
2
xTAx min{A} x
25
Spectral norm
26
Fixed points
Consider a continuous function f : IRn IRn. The vector x IRn is a xed point of f (x) if f (x ) = x . If x is a xed point of the function f (x), then x is a solution of f (x) x = 0. The Contraction Mapping Theorem provides a sucient condition for the existence and unicity of xed points.
27
f : IRn IRn x f (x , ) where stands for a vector of parameters. Assume that there exists a nonnegative constant k such that f (x , ) f (y , ) k x y for all x, y IRn and all .
If k < 1, then for each , the function f (x, ) possesses a unique xed point x IRn.
R. Kelly, V. Santiba nez and A. Loria 28
Lyapunov stability
Second method of Lyapunov or direct method of Lyapunov. Dynamical systems described by = f (t, x), x where x corresponds to the state. We will use x(t) to denote a solution to (1) in place of x(t, t, x(t)).
R. Kelly, V. Santiba nez and A. Loria 29
x IRn, t IR+ ,
(1)
We assume that f : IR+ IRn IRn is continuous in t and x and is such that it has a unique solution corresponding to each initial condition t, x(t) and the solution x(t, t, x(t)) depends continuously on the initial conditions t , x ( t ) . If f (t, x) = f (x) then, is said to be autonomous. we can safely consider that t = 0. f (t, x) = A(t)x + u(t) is linear dierential equation In the opposite case it is nonlinear
R. Kelly, V. Santiba nez and A. Loria 30
Denition 2.1 A constant vector xe IRn is an equilibrium or equilibrium = f (t, x), if state of the system x f (t, xe) = 0 t 0.
= f (t, x). If this is not the Typically x = 0 IRn is an equilibrium of x case, this may be translated to the origin.
31
x1 1 x (t) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . .. . . . . . . . . . . . . . . .
x 62 x(0) = xe
x 62 x(0) = xe t
. . . .. . . .. . . .. . . .. . . . . . . . . . . . . .
x1
Figure 9: Equilibrium If the initial state x(t) IRn is an equilibrium (x(t) = xe IRn) then, x ( t) = x e ( t) = 0 x t t 0 t t 0 .
32
Example 2.2
Consider a pendulum whose dynamic model is Jq + mgl sin(q ) = (t)
l g m
where m is the mass of the pendulum J is the total moment of inertia about the joint axis l is the distance from its axis of rotation to the center of mass q is the angular position. In terms of the state [q q ]T , the dynamic model is given by q q d . = dt q J 1 [ (t) mgl sin(q )]
R. Kelly, V. Santiba nez and A. Loria 34
If (t) = 0, the equilibrium states are given by [q q ]T = [n 0]T for n = , 2, 1, 0, 1, 2, since mgl sin(n ) = 0. If (t) = such that   > mgl, there does not exist any equilibrium since there is no q IR such that = mgl sin(q ).
R. Kelly, V. Santiba nez and A. Loria 35
Denitions of stability
x (t ) x1 x2 1 6 . . . . .. . . . .. . . . . . . .. . . .. . . . . . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . . . . . .. . . . . .. . . .. . . .. . . . . . . . . .. . . . . . . . . .. . . .. . .. . . .... .. . . .. .. . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . .. .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x ( t ) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x . . . . . . . . . . . . . . e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . t . . . . . . . . . . . . .. . . . . . .. . . . . . . . . . . . . . . . . . . . . . .
. . . . . . .. . . . . . . . . . . . . .. .. . . . . . . . . . . . . . .. . .. . . . . . . . . . . .. .. . . .. . .. . . . . . . . . . . .. .. . . . . . . . . . . .. .. .. .. . . . . . . . . . . . . .. .. . . . . . . . .. . . .. . . . . . . . . .. . . . .. . . .. . . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . .. .
x (t ) x2 6 . . . . . . .. .. . . @ . . .. .. . . . . .. .. . . . . . . . . . . . @ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . R ... ...@ . . . .. . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . .. . .. . . . . . . . . . . .. . . .. . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . .. . . . x1 . . . . . . . . . . . . . . . . . . . . . . .. . . .
. . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . .. .. . . . . . .. . . . . . . . . . . .. . . . . . . .. .. . . . .. . . .. . . . .. . . ..
Figure 11: Stability Denition 2.2 Stability. The origin is a stable equilibrium (in the sense of Lyapunov) of Equation (1) if, for each pair of numbers > 0 and t 0, there exists = (, t) > 0 such that x(t) < = x ( t) < t t 0 . (2)
36
Example 2.3
. . . . . ............... . . . . . . . . . . . . . . . ....... . . . . . . . . . ... .. .... . .. ........ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... .... . . . . . . . . . x . . . . e . . . . . . . . ... .................. . ..... . ... . . ........ . . . .. ............ ............... . . . .
x2
x1
Figure 12: Harmonic oscillator: Phase plane System described by the equations: x 1 = x2 x 2 = x1 The origin is the unique equilibrium point and is stable.
R. Kelly, V. Santiba nez and A. Loria 37
Denition 2.3 Uniform stability. The origin is a uniformly stable equilibrium (in the sense of Lyapunov) of Equation (1) if for each number > 0 there exists = () > 0 such that (2) holds.
38
x2 6 .. . . . . .. .. . . .
Figure 13: Asymptotic stability Denition 2.4 Asymptotic stability. The origin is an asymptotically stable equilibrium of (1) if 1. the origin is stable and 2. the origin is attractive, i.e., for each t, there exists = (t) > 0 : x ( t ) <
R. Kelly, V. Santiba nez and A. Loria
x(t) 0 as
t , t 0 .
(3)
39
. . . . ...................... . . . . . . .............. .... . . . . . . . ....... ... . . ............... . . . . . . . . . . . . . . . .... . . . .. ... .. . ........... . . ... .... . .. . . ... ........ . . . . . . . .... ....... .. . .......... ... .... .. .... ............. . . . . . ................... . .. . ........ . .. . ....... ... . . . . . ..... ... . . . . . . ..................... ..... ..... ..... ..... ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . .. ..... .... .... .... .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . .... .......... . . . . . . . ..... ... .. . . .. . . . .. .. .. ......... .... .... ..... . . ...... ... .. .. . . . . . ...... ........ .... . . . . . . . . . ............................ . ............................................. . .. . . . . ................... . . . . . . . . . . . . . . . . . . . . . .
x2
x1
Figure 14: Attractive but unstable equilibrium (x1 = rcos() and x2 = r sin()) Example 2.4 5 r = r(1 r) 100 = sin2(/2) [0, 2 ). Equilibria at the origin [r ]T = [0 0]T and at [r ]T = [1 0]T .
R. Kelly, V. Santiba nez and A. Loria 40
Denition 2.5 Uniform asymptotic stability. The origin is an uniformly asymptotically stable equilibrium of (1) if 1. the origin is uniformly stable and 2. the origin is uniformly attractive, that is, there exists a number > 0 such that (3) holds with a rate of convergence independent of t.
41
Denition 2.6 Global asymptotic stability. The origin is a globally asymptotically stable equilibrium 1. the origin is stable and 2. the origin is globally attractive, that is, x(t) 0 as t , x(t) IRn, t 0.
42
Denition 2.7 Global uniform asymptotic stability. The origin is a globally uniformly asymptotically stable equilibrium of Equation (1) if: 1. the origin is uniformly stable with () in (2) which satises () as (uniform boundedness) and 2. the origin is globally uniformly attractive, i.e., for all x(t) IRn and all t 0 , x(t) 0 as t with a convergence rate that is independent of t.
R. Kelly, V. Santiba nez and A. Loria 43
Denition 2.8 Global exponential stability. The origin is a globally exponentially stable equilibrium of (1) if there exist positive constants and , independent of t, such that x(t) < x(t) e (tt), t t 0 , x(t) IRn .
44
. . ......... . . . . . . .. .. . . .. .. . . . . . . . ........... .. .. .. .. . .. .. . . . .. . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . .. . . . . . . . . . . . . . . . . .. . . . . . .. . .. . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ...... ..... ..... .. ..... ..... ..... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ........ . . . ..... .... . . . . . . . . . .. . . . . . . . .. . ....... .. . . . . ......... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. ... . ... . .. . . .... . . . ......... ................ ... . ........................ . . . . . .. . ..... . . ....... .............. ............. . . .............. . . .
x2
x1
Figure 15: Van der Pol: Phase plane. Denition 2.9 Instability. The origin of Equation (1) is unstable if it is not stable. Example 2.6 Van der Pol system, x 1 = x2 , x 2 = x1 + (1 x2 1 )x 2 .
R. Kelly, V. Santiba nez and A. Loria 45
Lyapunov functions
Denition 2.10 Locally and globally positive denite function. A continuous function W : IRn IR+ is said to be locally positive denite if 1. W (0) = 0, 2. W (x) > 0 for small x = 0.
It is said to be globally positive denite (or simply positive denite) if 1. W (0) = 0, 2. W (x) > 0 x = 0.
46
W (x) is said to be (locally) negative denite if W (x) is (locally) positive denite. A function V : IR+ IRn IR+ is (resp. locally) positive denite if there exists a positive denite function W : IRn IR+ such that 1. V (t, 0) = 0 t 0 2. V (t, x) W (x), t 0, x IRn (resp. for small x ).
47
Denition 2.11 Radially unbounded function and decrescent function. A continuous function W : IRn IR is said to be radially unbounded if W (x ) as x .
V (t, x) is radially unbounded if V (t, x) W (x) for all t 0. A continuous function V : IR+ IRn IR is (resp. locally) decrescent if there exists a (resp. locally) positive denite function W : IRn IR+ such that V (t, x) W (x) t0 x IRn (resp. for small x ).
Example 2.7
Consider the graphs of the functions Vi(x) with i = 1, . . . , 4 as depicted in Figure 16. V1(x) is locally positive denite but is not (globally) positive denite. V2(x) is locally positive denite and (globally) positive denite. Also it is radially unbounded. V3(x) is locally positive denite and (globally) positive denite but it is not radially unbounded. V4(x) is positive denite and radially unbounded.
R. Kelly, V. Santiba nez and A. Loria 49
. . . . . . . . . . . . V ( x ) . . . 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... ...... . . . . . . . . . . . . . .. . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . . . . ..... ..... .... .. . ... . .... .... . ... .. ..... ..... . .... . . ... ..... ..... ...0 . . . . . . . . . . . . . . . . . . . . . . . ... .... . . .
. . ( x ) V . . . 2 . . . . . ... . . . . .. . . . . . . . . . . . . . . .. . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . .. .. . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . ... . ... . . . . . ... . .... .... .... .... ..... ..... ..... ... ... ..... ..... ..... ..... ..... ..... .... . . . .
x 0
. . . . . . . ........................... . . ........................... .. . . . . . . .. . . . . . . . . . . ... . . . . .. . .... ..... ..... .... .... .... .... .... ..... .. ..... ..... ..... ..... ..... ..... ...0 . . . . .
x 0 V4 (x) . . . . . . . . . . .. .. . . . . . . .. . . .. . ... . ... . . .. ... . . . . ... . .... . ... . . ..... . . . . ........ . .... .... .... .... ..... ....... .. .. ....... ........ ..... ..... ..... ..... ..... .... . . . . . . x 0
V3 (x)
x 0
Denition 2.12 Lyapunov function candidate. A function V : IR+ IRn IR+ is said to be a Lyapunov function candidate = f (t, x) if for the equilibrium x = 0 IRn of the equation x 1. V (t, x) is locally positive denite, 2. 3.
V (t,x) t V (t,x) x
51
Denition 2.13 Time derivative of a Lyapunov function candidate. = f (t, x). Let V (t, x) be a Lyapunov function candidate for the equation x The total time derivative of V (t, x) along the trajectories of (1), denoted (t, x), is given by by V (t, x) := d {V (t, x)} = V (t, x) + V (t, x) f (t, x). V dt t x
T
52
Denition 2.14 Lyapunov function. A Lyapunov function candidate V (t, x) for Equation (1) is a Lyapunov function for (1) if its total time derivative along the trajectories of (1) satises (t, x) 0 V t0 and for small x .
53
for small x .
If moreover V (t, x) W2(x), t 0 (decrescent) for small x , then the origin is uniformly stable. W1(x) and W2(x) are positive denite functions.
R. Kelly, V. Santiba nez and A. Loria 54
Theorem 2.3 (Uniform) boundedness of solutions plus uniform stability. = f (t, x) and the solutions The origin is a uniformly stable equilibrium of x x(t) are uniformly bounded for all initial conditions (t, x(t)) IR+ IRn if there exists V (t, x) W1(x) > 0, t 0 x IRn
and V (t, 0) = 0 t 0 (positive denite condition) as x (radially unbounded) with W1(x) V (t, x) W2(x) (t, x) 0 V t0 x IRn (decrescent) x IRn .
t t 0
Example 2.11
Consider the dynamic model of an ideal pendulum without friction Jq + mgl sin(q ) = 0 or, in the statespace form x 1 = x2 mgl sin(x1), x 2 = J . where x1 = q and x2 = q Autonomous nonlinear equation
R. Kelly, V. Santiba nez and A. Loria 56
with
q (0), q (0) IR
x1
57
Lyapunov function candidate x2 V (x1, x2) = mgl [1 cos(x1)] + J 2 . 2 which is locally positive denite Total time derivative of V (x1, x2) (x1, x2) = mgl sin(x1)x V 1 + Jx2x 2 = 0. According with Theorem 2.2 the origin is a stable equilibrium.
58
Theorem 2.4 Global (uniform) asymptotic stability = f (t, x) is globally asymptotically stable if there exists The origin of x V (t, x) W1(x) > 0, t 0 x IRn
and V (t, 0) = 0 t 0 (positive denite condition) with W1(x) as x (radially unbounded) (t, x) W3(x) < 0 V t0 x IRn .
(t, 0) = 0 t 0 (negative denite condition) and V If moreover V (t, x) W2(x) t 0 x IRn (decrescent), the origin is globally uniformly asymptotically stable. W1(x), W2(x) and W3(x) are positive denite functions.
R. Kelly, V. Santiba nez and A. Loria 59
Theorem 2.5 Global exponential stability. = f (t, x) is globally exponentially stable if there exists a The origin of x Lyapunov function candidate V (t, x) and positive constants , , and p 1 such that: x
p
V (t, x) x ,
p
(t, x) x V
t t 0
x IRn .
If all the above conditions hold only for small x then we say that the origin is an exponentially stable equilibrium.
60
Theorem 2.6 Consider the dierential equation = f (t, x), x = f (x ), x x IRn, x IRn. t IR+
The unicity of an existing equilibrium point is necessary for the following properties (or, in other words, the following properties imply the unicity of an equilibrium) Global asymptotic stability, Global exponential stability.
R. Kelly, V. Santiba nez and A. Loria 61
Theorem 2.7 La Salle = f (x), whose origin Consider the autonomous dierential equation x x = 0 IRn is an equilibrium. Assume that there exists a globally positive denite and radially unbounded Lyapunov function candidate V (x), such that (x ) 0 V x IRn.
(x) = 0 . If Dene the set = x IRn : V x(0) = 0 is the only initial state in , such that x(t) for all t 0, then the origin x = 0 IRn is globally asymptotically stable.
R. Kelly, V. Santiba nez and A. Loria 62
Corollary 2.1 Simplied La Salle. Consider the set of autonomous dierential equations = f x (x , z ) , x z = f z (x , z ) , x IRn z IRm .
where f x(0, 0) = f z (0, 0) = 0. That is, the origin is an equilibrium point. Let V : IRn IRm IR+ be globally positive denite and radially unbounded in both arguments. Assume that there exists a globally positive denite function W : IRm IR+ such that (x , z ) = W (z ) . V If x = 0 is the only solution of f z (x, 0) = 0 then the origin [xT z T ]T = 0 is globally asymptotically stable.
R. Kelly, V. Santiba nez and A. Loria 63
Lemma 2.2 Consider the continuously dierentiable functions x : IR+ IRn, z : IR+ IRm, h : IR+ IR+ and P : IR+ IR(n+m)(n+m). Assume P (t) = P (t)T > 0 for each t IR+. T x x Dene: V (t, x, z , h) = P (t) + h(t) 0 . z z T x (t, x, z , h) = Q(t) If V 0 z x 0 0 0 z
for all t IR+, x IRn, z IRm and h IR+, where Q(t) = Q(t)T > 0 for all t 0, then,
R. Kelly, V. Santiba nez and A. Loria 64
1. x(t), z (t) and h(t) are bounded for all t 0 and 2. x(t) is squareintegrable, i.e.,
x ( t)
0
dt < .
65
zn joint n link n
y0 x0
The generalized joint coordinate qi is the angular displacement around zi (revolute) or linear displacement along zi (prismatic).
z1 z0 z2 q3 z4 z3
q2 q1 q4
x= x1 x2 x3
y0 x0
The vector of joint positions q has n elements q1 q2 n q= IR . . . qn The position and orientation of the robots endeector, are collected in the vector x of operational positions . x1 x2 x= . . xm where m n.
R. Kelly, V. Santiba nez and A. Loria 68
The direct kinematic model describes the relation betwwen q and x. It is a function : IRn IRm such that x = (q ). The inverse kinematic model consists on the inverse relation of the direct kinematic model q = 1(x). The computation may be highly complex.
69
The dynamic model of a robot consists of an ordinary dierential equation. In general, these are second order nonlinear models ,q , ) = 0 , f EL(q , q , x, ) = 0 . f C (x , x stands for the forces and torques applied. The dynamic model (4) is called joint dynamic model, while (5) corresponds to the operational dynamic model. We focus on the joint dynamic model.
R. Kelly, V. Santiba nez and A. Loria 70
(4) (5)
zn joint n link n
y0 x0
) is dened as The Lagrangian L(q , q ) = K (q , q ) U (q ) . L (q , q where ) is the kinetic energy function K (q , q U (q ) is the potential energy function (we assume only conservative forces) The total energy E of a robot manipulator of n DOF is ) = K (q , q ) + U (q ) E (q , q where q = [q1, , qn]T .
R. Kelly, V. Santiba nez and A. Loria 72
(6)
The equations of motion of Lagrange for a manipulator of n DOF, are given by ) ) d L(q , q L(q , q = , dt q q or in the equivalent form by ) ) d L(q , q L(q , q = i, dt q i qi where i correspond to the external as well as to other non conservative forces and torques at each joint.
R. Kelly, V. Santiba nez and A. Loria 73
i = 1, , n
(7)
The use of Lagranges equations in the derivation of the robot dynamics can be reduced to four main stages: ). 1. Computation of the kinetic energy function K(q , q 2. Computation of the potential energy function U (q ) . ). 3. Computation of the Lagrangian (6) L(q , q 4. Development of Lagranges equations (7).
74
Example 3.3
q2 lc2
m2 I2
where, l1 is the length the rst link m1 and m2 are the masses of the links lc1 and lc2 are the distances of the rotating axes to the centers of their respective mass I1 and I2 denote the moments of inertia of the links q = [q1 q2]T denes the vector of joint positions Notice that the center of mass of link 2 may be physically placed out of the link itself! This is determined by the value of the constant angle
R. Kelly, V. Santiba nez and A. Loria 76
) may be decomposed by The kinetic energy function K(q , q ) + K2(q , q ), ) = K1(q , q K (q , q where ) is the kinetic energy associated to the mass m1 K1(q , q ) is the kinetic energy associated to the mass m2 K2(q , q with 1 2 2 2 ) = 1 K1(q , q m l q + 1 1 c1 1 2 2 I1 q ) = K2(q , q
m2 2 2 m2 2 2 l q + 2 2 1 1 2 lc2 q 2 +1 2 . 2 I2 q
+ m2l1lc2 cos(q1 q2 + )q 1q 2
77
78
d dt
) L(q ,q q i
) L(q ,q qi
= i,
i = 1, , n
1 =
2 2 m1lc + m l 1 2 1 1 + I1 q
+ m2l1lc2 cos(q1 q2 + ) q2
2 + m2l1lc2 sin(q1 q2 + )q 2
(9)
R. Kelly, V. Santiba nez and A. Loria 80
where RHS(8) and RHS(9) denote the terms on the right hand side of (8) and (9) respectively.
81
Example 3.4
Consider the 3DOF Cartesian robot manipulator shown in Figure 22.
z0 q2 z0 m3 q3 q1 y0 x0 x0 q2 m1 m2
q1 y0 q3
U (q ) = [m1 + m2 + m3]gq1 .
R. Kelly, V. Santiba nez and A. Loria 82
We obtain the Lagrangian as: ) = K (q , q ) U (q ) L(q , q 1 2 2 2 m1q = 3 + [m1 + m2]q 2 + [m1 + m2 + m3]q 1 2 [m1 + m2 + m3]gq1 ,
hence, the dynamic equations result q1 + [m1 + m2 + m3]g = 1 [m1 + m2 + m3] [m1 + m2] q2 = 2 m1q 3 = 3
83
q1
q 1
The necessary and sucient condition for the existence of equilibria is 1 = [m1 + m2 + m3]g , 2 = 0 and 3 = 0 Actually we have an innite number of them:
1 q 2 q 3] = [q1 q2 q3 0 0 0] [q1 q2 q3 q T T
with q1 , q2 , q3 IR.
R. Kelly, V. Santiba nez and A. Loria 85
where M (q ) = M (q )T > 0 for all q IRn is the n n inertia matrix. U (q ) denotes the potential energy The Lagrangian results 1 T ) = q U (q ) . M (q )q L(q , q 2
R. Kelly, V. Santiba nez and A. Loria 86
The Lagranges equations of motion ) ) L(q , q d L(q , q = , dt q q may be written as d 1 T M (q )q q 2 dt q which takes the compact form, )q + g (q ) = M (q ) q + C (q , q 1 T U (q ) + M (q )q =. q q 2 q
87
where )q C (q , q 1 (q )q TM (q )q = M q 2 q U (q ) . g (q ) = q
)q is a vector of dimension n called vector of centrifugal and C (q , q Coriolis forces, g (q ) is a vector of dimension n of gravitational forces or torques and is a vector of dimension n called the vector of external forces
R. Kelly, V. Santiba nez and A. Loria 88
) IRnn may be not unique, but C (q , q )q is indeed unique. C (q , q ) is through the Christoel symbols One way to obtain C (q , q cijk (q ) = 1 Mkj (q ) Mki(q ) Mij (q ) + . 2 qi qj qk
), is given by ), Ckj (q , q The kj th element of the matrix C (q , q c1jk (q ) c2jk (q ) q ) = Ckj (q , q . . . cnjk (q )
R. Kelly, V. Santiba nez and A. Loria 89
ROBOT q
90
Example 3.6
z x l1 q1 I1 m1 1 lc1
q2 lc2
m2 I2
91
The dynamic model, for this robot, in compact form is ) C12(q , q ) C11(q , q M11(q ) M12(q ) + q q ) C22(q , q ) M21(q ) M22(q ) C21(q , q M (q ) where M11(q ) =
2 2 m1lc + m l 2 1 1 + I1
= ( t)
) C (q , q
M12(q ) = [m2l1lc2 cos( )C21 + m2l1lc2 sin( )S21] M21(q ) = [m2l1lc2 cos( )C21 + m2l1lc2 sin( )S21]
2 M22(q ) = [m2lc 2 + I2 ]
R. Kelly, V. Santiba nez and A. Loria 92
) = 0 C11(q , q ) = [m2l1lc2 cos( )S21 + m2l1lc2 sin( )C21]q 2 C12(q , q ) = [m2l1lc2 cos( )S21 m2l1lc2 sin( )C21]q C21(q , q 1 ) = 0 C22(q , q That is, the gravitational forces vector is zero.
93
)q + g (q ) = in terms of the state The dynamic model M (q ) q + C (q , q T ]T can be expressed as vector [q T q q q d . = dt )q g (q )] q M (q )1 [ (t) C (q , q
(10)
The necessary and sucient condition for the existence of equilibria is (t) be constant (say, ) and there exist a solution q IRn to g (q ) = . T ]T = [q T 0T ]T IR2n. The equilibria are given by [q T q
R. Kelly, V. Santiba nez and A. Loria 94
T ]T = When 0, the possible equilibria of (10) are given by [q T q [q T 0T ]T where q is a solution of g (q ) = 0 U (q ) As g (q ) = q , we have that q corresponds to the vectors where the potential energy possesses extrema.
95
v2
v1
q1
Figure 25: 2DOF robot. On a real robot manipulator the torques vector , is delivered by actuators electromechanical, pneumatic or hydraulic.
R. Kelly, V. Santiba nez and A. Loria 96
v2
q2
q1
v1
Figure 27: Diagram of a DC motor. Simplied linear dynamic model of a DC motor KaKb Ka q + 2 = v Ra r rRa
Jmq + fmq +
where:
R. Kelly, V. Santiba nez and A. Loria 98
Jm : rotors inertia [kg m2], Ka : motortorque constant [N m/A], Ra : armature resistance [], Kb : back emf [V s/rad], fm : rotors friction coecient with respect to its hinges [N m], : net torque applied after the set of gears at the loads axis [N m], q : angular position of the loads axis [rad], r : gear reduction ratio (in general r v : armature voltage [V].
R. Kelly, V. Santiba nez and A. Loria 99
1),
We can obtain + Bq + R = K v Jq with J = diag {Jmi } KaKb B = diag fmi + Ra 1 R = diag 2 ri Ka 1 K = diag Ra i ri
100
The complete dynamic model of a manipulator (considering friction in the joints) including its actuators is + R C (q , q )q + R g (q ) + R f (q ) + B q = K v . (11) (R M (q ) + J ) q
v ACTUATORS
ROBOT q
q q
101
Example 3.8
Consider the pendulum depicted in Figure 29.
fL lb Jm v fm Ka , Kb , Ra
1:r
mb , J m q
Figure 29: Pendular device with a DC motor. The equation of motion for this device including its load is given by + [mblb + ml] g sin(q ) = Jq + fLq
R. Kelly, V. Santiba nez and A. Loria 102
where J : arms inertia without load (i.e., with m=0), mb : arms mass (without load), lb : distance from the rotating axis to the arms center of mass (without load), m : loads mass (assumed to be punctual), l : distance from the rotating axis to the load m, g : gravity acceleration, : applied torque at the rotating axis, fL : friction coecient of the arm and its load.
R. Kelly, V. Santiba nez and A. Loria 103
The equation above may also be written in the compact form + fLq + kL sin(q ) = JLq
104
The complete dynamic model of the pendular device may be obtained as: Jm + JL fL KaKb Ka kL + + sin( q ) = v, q + f q + m 2 2 2 r r Ra r rRa
105
When the gear ratios ri are suciently large, the robotwithactuators equation (11) may be approximated by + Bq Kv . Jq
In the opposite case, the equation (11) may be rewritten as )q + g (q ) + f (q ) + R1B q = R1K v M (q ) q + C (q , q where M (q ) = M (q ) + R1J .
106
q q d . = dt C (q , q )q g (q ) f (q ) q M (q )1 R1 K v R1 B q The problem of motion control is: d and q d, given q d, q determine v , to be applied to the motors, in such a manner that q follow precisely q d .
107
where I denotes the identity matrix of dimension n n. The matrix M (q )1 exists and is positive denite.
109
For robots having only revolute joints there exists a constant > 0 such that Max{M (q )} q IRn . One way of computing is 2. n max Mij (q )
i,j,q
110
For robots having only revolute joints there exists a constant kM > 0 such that 3. M (x)z M (y)z kM xy z
for all vectors x, y , z IRn. One simple way to determine kM is as follows. Mij (q ) . kM n2 max i,j,k,q qk
111
For robots having only revolute joints there exists a number kM > 0 such that 4. for all x, y IRn. M (x)y kM y
112
) Property 4.2 Coriolis matrix C (q , q ) IRnn satises the The centrifugal and Coriolis forces matrix C (q , q following.
1.
) may be not unique For a given manipulator, the matrix C (q , q )q is unique. but the vector C (q , q
2.
113
For all vectors q , x, y , z IRn and scalars we have that 3. C (q , x )y = C (q , y )x C (q , z + x)y = C (q , z )y + C (q , x)y .
114
The vector C (q , x)y may be written in the form xTC1(q )y T C (q , x )y = x C 2 (q )y . . xTCn(q )y where for all matrix of the
1 2
4.
Ck (q ) are symmetric matrices of dimension n n k = 1, 2, , n. The ij th element Ckij (q ) of the Ck (q ) corresponds to the socalled Christoel symbol rst kind cjik (q ) and which is dened as cijk (q ) = Mkj (q ) Mij (q ) Mki (q ) . + qi qj q
k
115
For robots having exclusively revolute joints, there exists a number kC1 > 0 such that 5. for all q , x, y IRn. C (q , x)y kC1 x y
116
For robots having exclusively revolute joints, there exist numbers kC1 > 0 and kC2 > 0 such that 6. C (x , z )w C (y , v )w kC1 z v +kC2 x y for all vector v , x, y , z , w IRn. w w z
117
), dened using the Christoel symbol is related to The matrix C (q , q the inertia matrix M (q ) by the expression x 7.
T
1 ) x = 0 M (q ) C (q , q 2
, x IRn q, q
and as a matter of fact, 1 2 M (q ) C (q , q ) is skewsymmetric. Equivalently, (q ) 2 C (q , q ) is antisymmetric, and it is also true that the matrix M (q ) = C (q , q ) + C (q , q )T . M
118
1 ) q =0 M (q ) C (q , q 2
IRn . q, q
119
1.
0
120
For robots having only revolute joints there exists a number kU such that 2.
0 T
(t) dt + U (q (0)) kU g (q (t))Tq for all T IR+ and where kU = min {U (q )}.
q
121
For robots having only revolute joints, the vector g (q ) is Lipschitz, that is, there exists a constant kg > 0 such that g (x) g (y ) kg x y 3. for all x, y IRn. A simple way to compute kg is by evaluating its partial derivative gi(q ) kg n max . i,j,q qj
122
123
4.
For robots having only revolute joints there exists a constant k such that g (q ) k for all q IRn.
124
Residual dynamics
) is dened as follows. The residual dynamics h( q, q ) = [M (q d) M (q d q )] q d h( q, q ) q d ) C (q d q , q d q d + C (q d , q ), + g (q d ) g (q d q and with an abuse of notation it may be written as ) = [M (q d) M (q )] q d + [C (q d, q d ) C (q , q )] q d + g (q d ) g (q ). h( q, q
125
(12)
0.5 1.0
4 3 2 1
3 x 4
The rst partial derivative of tanh(x) is given by tanh (x) =: Sech2(x) = diag{sech2(xi)} x where sech(xi) := 1 . x x i i e e
127
The vectorial tangent hyperbolic function satises the following properties. IRn For any x, x tanh(x) 1 x tanh(x) 2 tanh(x)
2
3 tanh(x)T x
4 x Sech2(x)x
) Property 4.4 Vector residual dynamics h( q, q ) of n 1 depends on The vector of residual dynamics h( q, q , , q q d, and q d (supposed to be bounded) q d, q d M the supreme values over the norms d M and q We denote by q of the desired velocity and acceleration. ) has the following property: In addition, h( q, q
129
There exist constants kh1, kh2 0 such that the norm of the residual dynamics satises 1. ) kh1 q + kh2 tanh( h( q, q q) IRn, where tanh( , q q ) is the vectorial tangent for all q hyperbolic function introduced in the latter Denition.
130
where kh2
s2 s2 tanh s1
d kC1 q with
kh1 ,
d s1 = kg + kM q and d s2 = 2 k + kM q
R. Kelly, V. Santiba nez and A. Loria
d M + kC2 q
2 M
d M + kC1 q
2 M
.
131
Figure 31: Pelican: experimental prototype from CICESE, Robotics lab. The prototype is a vertical planar manipulator two links connected to revolute joints.
R. Kelly, V. Santiba nez and A. Loria 132
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Figure 32: Diagram of the Pelican prototype robot with 2 degrees of freedom.
R. Kelly, V. Santiba nez and A. Loria 133
l1 l2 lc1 lc2 m1 m2 I1 I2 g
134
The chapter is organized as follows: Direct kinematics. Inverse kinematics. Dynamic model. Model properties of the dynamic model. Reference trajectories.
R. Kelly, V. Santiba nez and A. Loria 135
Direct kinematics
For this case, the problem consists on expressing x = (q 1 , q 2 ) , y where : IR2 IR2. The direct kinematic model is given by x = l1sin(q1) + l2sin(q1 + q2) y = l1cos(q1) l2cos(q1 + q2) .
136
It may also be obtained the relation between the velocities, x y = l1cos(q1) + l2cos(q1 + q2) l2cos(q1 + q2) l1sin(q1) + l2sin(q1 + q2) l2sin(q1 + q2) q 1 q 2 q 1 q 2
= J (q ) where
137
q 1 q 1 + J (q ) q 2 q 2
138
Inverse kinematics
q1 = 1(x, y ), where 1 : IR2 and IR2. q2
y
qd 1
qd 2
Figure 33: Two solutions to the inverse kinematics problem. It may have multiple solutions or no solution at all!
R. Kelly, V. Santiba nez and A. Loria 139
qd1 x
x qd1
A practical interest relies on its utility to dene q d = [qd1 qd2 ]T from specied desired positions xd and yd. q d1 = 1(x, y ). q d2 q d1 q d2 = tan1 = cos
1
xd yd
tan1
l2sin(qd2 ) l1 + l2cos(qd2 ) .
2 2 2 x2 d + yd l1 l2 2 l1 l2
= J 1(q d)
x d y d
141
d x d x d 1 1 J (q d ) J (q d ) (q d ) + J (q d ) y d y d dt d 1 J (q d ) dt
S12 C12 l S l S 1 2 1 2 1 J (q d ) = l1S1 l2S12 l1C1 + l2C12 , l1l2S2 l1l2S2 and d1 l2S12(q d1 + q d2 ) l2S12(q d1 + q d2 ) l1S1q d , [J (q d)] = dt l 1 C1 q d1 + l2C12(q d1 + q d2 ) l2C12(q d1 + q d2 )
R. Kelly, V. Santiba nez and A. Loria 142
where
where for simplicity S1 = sin(qd1 ), S2 = sin(qd2 ), C1 = cos(qd1 ), S12 = sin(qd1 + qd2 ), C12 = cos(qd1 + qd2 ) . Typically, singular congurations are those in which the endeector of the robot is located at the physical boundary of the workspace
y qd2
x qd1
143
Dynamic model
where ) is the kinetic energy associated to the mass m1 K1(q , q ) is associated to the mass m2 K2(q , q
R. Kelly, V. Santiba nez and A. Loria 144
with ) = K1(q , q = and ) = K2(q , q = 1 1 T m2v 2 v 2 + I2(q 1 + q 2)2 2 2 m2 2 2 m2 2 2 2 l1 q lc2 q 1 + 1q 2 + q 2 1 + 2q 2 2 2 + m2l1lc2 q +q 1q 2 cos(q2) 1 1 + I 2 (q 1 + q 2)2. 2
R. Kelly, V. Santiba nez and A. Loria 145
Similarly the potential energy is given by U (q ) = U 1 (q ) + U 2 (q ) where, assuming that the potential energy is zero at y = 0, we obtain that U1(q ) = m1lc1g cos(q1) U2(q ) = m2l1g cos(q1) m2lc2g cos(q1 + q2) .
146
The Lagrangian is ) = K (q , q ) U (q ) L(q , q 1 1 2 2 2 2 2 2 1 [m1lc1 + m2l1 ]q = 1 + m2lc + 2q 1q 2 + q 2 2 q 2 2 2 1 +q 1q 2 + m2l1lc2 cos(q2) q + [m1lc1 + m2l1]g cos(q1) + m2glc2 cos(q1 + q2) 1 2 1 + I1 q 1 + I2[q 1 + q 2]2. 2 2
d dt L q i L q = i i
i = 1, 2 we get
147
1 =
2 2 2 m1lc + m l + m l 1 2 2 1 1 c2 + 2m2 l1 lc2 cos(q2) + I1 + I2 q 2 2 + m2lc 2 + m2 l1 lc2 cos(q2 ) + I2 q 2 2m2l1lc2 sin(q2)q 1q 2 m2l1lc2 sin(q2)q 2
148
Model in compact form ) C12(q , q ) C11(q , q g (q ) M11(q ) M12(q ) + + 1 q q ) C22(q , q ) M21(q ) M22(q ) C21(q , q g 2 (q ) M (q ) ) C (q , q g (q ) = ,
149
where
2 2 2 M11(q ) = m1lc + m + l l 2 1 1 c2 + 2l1 lc2 cos(q2) + I1 + I2 2 M12(q ) = m2 lc 2 + l1 lc2 cos(q2) + I2 2 M21(q ) = m2 lc 2 + l1 lc2 cos(q2) + I2 2 M22(q ) = m2lc 2 + I2
) = m2l1lc2 sin(q2)q C11(q , q 2 ) = m2l1lc2 sin(q2) [q C12(q , q 1 + q 2] ) = m2l1lc2 sin(q2)q 1 C21(q , q ) = 0 C22(q , q g1(q ) = [m1lc1 + m2l1] g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) .
R. Kelly, V. Santiba nez and A. Loria 150
In terms of these state variables, the dynamic model of the robot may be written as q1 q 1 d q2 = q 2 . q dt 1 1 )q g (q )] M ( q ) [ (t) C (q , q q 2 The formulas and numeric values of the constants Max{M }, kM , kC1 , kC2 and kg are summarized in Tables below
151
kM
n2
i,j,k,q
max
Mij (q) qk
k C1
n2
k C2
i,j,k,l,q
max
Ckij (q ) ql
kg
max
gi (q ) i,j,q qj
152
Max {M } kM k C1 k C2 kg
kg m2 kg m2 kg m2 kg m2 kg m2 /sec2
153
] + c1[1 e
2.0 t3
3
] sin(1t) ] sin(2t)
[rad] (13)
b2[1 e2.0
] + c2[1 e2.0
where b1 = /4 [rad], c1 = /9 [rad] and 1 = 4 [rad/sec], and b2 = /3 [rad], c2 = /6 [rad] and 2 = 3 [rad/sec].
R. Kelly, V. Santiba nez and A. Loria 154
The gure depicts the graphs of above reference trajectories against time.
2.0 1.5 1.0 0.5 0.0
[rad]
.. d2 ...... ...... ...... ... .... . . . .. . .. . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . q d1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ...... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . .. ... . ... .. ... . . ... .. ... . ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... .... .... .... .... .... . ...... . . . . .. .. .. . . ..... ..... ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
10 t [sec]
155
....... . ....... . . . . . . . . . . . ....... ......... . . . . . . . . . .... ..... . . . . . . . . ... . . . . . . . . .... . . . . . ......... .. . ........ . . .. . . . ... .. . .. . . . . . . . . . . . . . . . . . . . . . . .. . . . . . ... .. . . . . . . . ...... ..... . . . . . . .... . . . . . . . . . .....
10 t [sec]
156
10 t [sec]
Figure 39: Norm of the desired reference velocities vector. q d1 = 6b1t e q d2 = 6b2t e
2 2.0 t3
+ 6c1t e + 6c2t e
2 2.0 t3
2.0 t3
2 2.0 t3
2 2.0 t3
2.0 t3
] cos (2t)2 .
in [ rad/sec ].
R. Kelly, V. Santiba nez and A. Loria
.. . . . . . . . . . . . . . ... ... ... ... ... ... . . . .. . . . . .. .. . .. ... ... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . ... . . . . . . .. . . . . . . ......... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . ... . ... ... . ... . ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10 t [sec]
158
36b1t e
t3
4 2.0 t3
+ 12c1te
2.0 t3
sen(1t)
36c1t e
4 2.0 t3
sen(1t) + 12c1t e
2 ]sen(1t)1
2 2.0 t3
cos (1t)1
[c1 c1e2.0
2.0 t3
rad / sec2 ,
2.0 t3
q d2 = 12b2te
36b2t e
4 2.0 t3
+ 12c2te
sin (2t)
36c2t e
4 2.0 t3
2 2.0 t3
cos (2t)2
[c2 c2e
2.0 t3
159
qd d q d q
160
161
Introduction
Introduction
Motion controllers are clasied Setpoint controllers (q d(t) = q d, is a constant vector), and tracking controllers (q d(t) is a time variant vector). )q + Consider the dynamic model of a robot manipulator M (q ) q + C (q , q g (q ) = , which can be written as: q q d = dt )q g (q )] q M (q )1 [ (t) C (q , q
162
Introduction
where M (q ) IRnn is the inertia matrix, )q IRn is the vector of centrifugal and Coriolis forces, C (q , q g (q ) IRn is el vector of gravitational forces and torques and IRn is a vector of external forces and torques applied at the joints. ,q IRn denote the position, velocity and joint The vectors q , q acceleration respectively.
163
Introduction
lim q (t) = q d
where q d IRn is a given constant vector which represents the desired setpoint. It is convenient to rewrite the set point control objective as
t
( t) = 0 lim q
IRn stands for the joint position errors where q (t) := q d q (t) . q
R. Kelly, V. Santiba nez and A. Loria 164
Introduction
In general, a control law may be expressed as ,q , q d , M (q ), C (q , q ), g (q )) . = (q , q However, for practical purposes, it is desirable that the controller does not . depend on the joint acceleration q
qd d q d q
CONTROLLER
ROBOT
q q
Introduction
A methodology to analyze the stability may be summarized in: 1. Derivation of the closed loop dynamic equation. 2. Representation of the closed loop equation in the statespace form, i.e., d qd q q dt ), g (q )) . , q d , M (q ), C (q , q = f (q , q
qd
CONTROLLER + ROBOT
q q
control
closedloop
system:
Inputoutput
166
Introduction
3. Study of the existence and possible unicity of the equilibrium for the closed loop equation. 4. Proposition of a Lyapunov function candidate to study the stability 5. Alternatively to step 4), determine the qualitative behavior of the solutions of the closed loop equation.
167
Introduction
The controllers that we present may be called conventional (commonly used in industrial robots). Velocity feedback Proportional control and Proportional Derivative (PD) control PD control with gravity compensation PD control with precalculated gravity compensation Proportional Integral Derivative (PID) control
168
Kv
Figure 43: Velocity feedback proportional control. Kv q , where Control law given by = Kpq Kp, Kv IRnn are symmetric positive denite matrices q d IRn corresponds to the desired joint position, = q d q IRn is called position error. q
R. Kelly, V. Santiba nez and A. Loria 169
ROBOT
q q
Kv d q qd
Kp
Figure 44: PD Control. , where + Kv q The PD control law is given by: = Kpq Kp, Kv IRnn are also symmetric positive denite = q (both control laws are identical). When q d, is constant, then, q
170
We assume a constant q d, then, the closed loop equation may be rewritten q q d . = dt Kv q C (q , q )q g (q )] q M (q )1 [Kpq It may have multiple equilibria given by q
T
T T
171
) = V ( q, q
1 T 1 T + q . M (q )q Kpq q 2 2
173
) results The total derivative of V ( q, q ( ) = q TKv q V q, q T 0 0 q q 0, = 0 Kv q q By Theorem 2.3 we conclude that the origin is stable and, (t) and q (t) are bounded. the solutions q By applying the La Salles theorem (Theorem 2.7) we have that
174
IRn, q = 0 IRn} , = {q (0)T q (0)T where q = 0 IR2n is the only initial condition in for which x(t) for all t 0. This is enough to establish global asymptotic stability of the origin, T T T q = 0 IR2n. q
T
175
(16)
The study of this section is limited to robots having only revolute joints. It may have multiple equilibria given by q
T
T T
176
Example 6.1
Consider the model of an ideal pendulum Jq + mgl sin(q ) = .
l g m
The equilibria expression takes the form kps mgl sin(qd s) = 0 . For the sake of illustration consider the following numeric values J =1 mgl = 1 kp = 0.25 qd = /2. The closed loop system under PD control has the equilibria q q 1.25 2.13 3.56 , , 0 0 0 .
178
Unicity of the equilibrium. The equilibria of the closed loop equation (16) satisfy T T q q
T
= sT 0T
, where
1 g (q d s ) = f (s , q d ) . s IRn is solution of s = Kp
If min{Kp} > kg we have (by contraction mapping theorem) the only equilibrium is q
T
= s
179
Arbitrarily bounded position and velocity error. Case where Kp is not restricted to satisfy min{Kp} > kg , but it is enough that Kp be positive denite. Dene the following nonnegative function 1 T 1 T + q ) = q + U (q ) kU 0 . M (q )q Kpq V ( q, q 2 2 where kU = minq {U (q )}
R. Kelly, V. Santiba nez and A. Loria 180
T Kv q = q T 0 0 q q 0. = 0 Kv q q Invoking Lemma 2.2, we conclude (t) and q (t) are bounded for all t, and q the velocities vector is square integrable, that is,
R. Kelly, V. Santiba nez and A. Loria
Moreover, the exact explicit bounds hold, for all t 0: (0)) 2V ( q (0), q min{Kp} (0) + q (0)TKpq (0)TM (q (0))q (0) + 2U (q (0)) 2kU q = min{Kp} (17) (0)) 2V ( q (0), q min{M (q )} (0)TM (q (0))q (0) + q (0)TKpq (0) + 2U (q (0)) 2kU q = min{M (q )} (18)
R. Kelly, V. Santiba nez and A. Loria 182
( t) q
( t) q
Example 6.2
Consider again the ideal pendulum from Example 6.1 Jq + mgl sin(q ) = ,
l g m
The potential energy function is U (q ) = mgl(1 cos(q )) and the constant kU is zero. Consider next the numeric values J =1 mgl = 1 kp = 0.25 kv = 0.50 qd = /2. Assume that we apply the PD controller to drive it from the initial conditions q (0) = 0 and q (0) = 0
184
According to the bounds (17) and (18), we get q 2(t) q 2(0) = 2.46 rad2 t0
3 2 1 0 0
q (t)2 [rad2 ]
..... ... ... ... ... ............ ...... ................................................................................................................ ... . . . . ....... ........ ..
10
15
20 t [sec]
rad sec
t0
.... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... . . . . . ... ...... . . . . . ..... ................... ......... . . ..... .... .......... .............................................................................. ..
10
15
20 t [sec]
186
That is, the solutions tend to one of the three equilibria determined in Example 6.1
187
Example 6.3
Consider the 2DOF prototype robot studied in Chapter 5,
y g
Link 1
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Figure 49: Diagram of the Pelican prototype robot with 2 degrees of freedom.
R. Kelly, V. Santiba nez and A. Loria 188
whose vector of gravitational torques g (q ) is g1(q ) = (m1lc1 + m2l1)g sin(q1) + m2glc2 sin(q1 + q2) g2(q ) = m2glc2 sin(q1 + q2). The control objective consists on making that lim q (t) = q d = /10 /30 [rad].
It may be veried that g (q d) = 0 T T q = 0 IR4 of the closed loop equation with the The origin q PD controller, is not an equilibrium.
R. Kelly, V. Santiba nez and A. Loria 189
190
[rad]
..... ... q ....1 .... ...... .......... ....................................................................................................... 0.1309 ......................... ..... q ...... 2 ................. 0.0174 .......................................................................................................... . . . . ..... . .... . . . .... ..... ..... ..... ..... ... .. ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... .... .... . . . .. .. .. .. . . .. . ..
0.1
0.0 0.5 1.0 1.5 2.0 t [sec]
Figure 50: Position errors q 1 and q 2. One may appreciate that limt q 1(t) = 0.1309 and limt q 2(t) = 0.0174 therefore, as it was expected, the control objective is not achieved
191
ROBOT
Figure 51: PD control with gravity compensation. + g (q ) where + Kv q The control law is given by = Kpq Kp, Kv IRnn are positive denite symmetric matrices.
R. Kelly, V. Santiba nez and A. Loria 192
Considering q d as constant, the closed loop equation may be written as q q d = dt )1 [Kpq , q )q ] Kv q C (q d q q M (q d q T q T The origin q
T
193
) = V ( q, q
1 T 1 T + q M (q )q Kpq q 2 2
194
Its total derivative with respect to time is ( ) = q TKv q V q, q T 0 0 q q 0 = 0 Kv q q ( ) 0 because Kv > 0 V q, q Consequently, by Theorem 2.3 the origin is stable and all the solutions (t) and q (t) are bounded. q
195
By applying the La Salles theorem (Theorem 2.7) we have that the set is given by = = (x ) = 0 x IR2n : V q x= q ( ) = 0 q, q IR2n : V
IRn, q = 0 IRn} , = {q (0)T = 0 IR2n is the only initial condition in (0)T q where q for which x(t) for all t 0. This is enough to establish global asymptotic stability of the origin, T T q T = 0 IR2n. q (t) = 0 and limt q (t) = 0 (the setpoint control Hence, limt q objective is achieved).
R. Kelly, V. Santiba nez and A. Loria 196
Example 7.1
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
The components of the vector of gravitational torques g (q ) are given by g1(q ) = (m1lc1 + m2l1)g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) . Consider the PD control with gravity compensation, where Kp = diag{kp} = diag{30} [Nm/rad] Kv = diag{kv } = diag{7, 3} [Nm sec/rad] .
198
The initial conditions are chosen as q1(0) = 0, q2(0) = 0 q 1(0) = 0, q 2(0) = 0 , and the desired joint positions as qd1 = /10, qd2 = /30 [rad] , hence, the initial state is taken to be 0.3141 /10 (0) q /30 0.1047 = 0 = 0 . (0) q 0 0
199
[rad]
..... ... ... q .... 1 .... .... ...... ........ ..... q ................. ...... 2 0.0620 ................................................................................... .......................... .............. ............................................................................................................. 0.0144 . .. . . . . . .... . . .. .. .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ... . ..... . . .... . . . .. . .. .. .
0.1
0.0 0.5 1.0 1.5 2.0 t [sec]
Figure 53: Position errors q 1 and q 2. From Figure above, steady state position errors due to to unmodelled friction phenomenon, can be seen.
200
0.5 1.0
4 3 2 1
.
201
IRn q
IRn . q
202
We present now an alternative stability analysis. Consider the Lyapunov function candidate 1 T 1 T ) = q + q tanh( M (q )q Kpq V ( q, q q )T M (q )q 2 2 where > 0 is a constant suciently small so as to satisfy simultaneously, min{Kp}min{M } 2 > 2 Max {M } (20) (19)
4min{Kp}min{Kv } >. (21) 2 { K } + 4 { K } [ n k + { M } ] v min p C1 Max Max There always exists > 0 arbitrarily small satisfying (20) and (21). ) > 0 because Kp > 0 and satises (20). V ( q, q
R. Kelly, V. Santiba nez and A. Loria 203
The time derivative of the Lyapunov function candidate yields (19). ( tanh( + q T Sech2( ) = q T Kv q q )T M (q )q q )T Kpq V q, q )T q tanh( , q )T C (q , q + tanh( q )T Kv q and is upperbounded by ( ) V q, q where min{Kp} Q= 1 2 Max {Kv }
R. Kelly, V. Santiba nez and A. Loria
tanh( q) q
tanh( q) q
Finally, Theorem 2.4 allows to establish global asymptotic stability of the origin.
205
Kv d q qd
Kp
Figure 55: PD Control with desired gravity compensation. + g (q d), where + Kv q Control law given by = Kpq Kp, Kv IRnn are symmetric positive denite matrices g (q d) may be evaluated oine, it is not necessary anymore to evaluate g (q ) in real time.
R. Kelly, V. Santiba nez and A. Loria 206
Considering the desired position q d to be constant, the closedloop equation may be written as q q d = dt Kv q C (q , q )q + g (q d) g (q )] q M (q )1 [Kpq T q T q
T
(22)
, may have the equation There are as many equilibria as solutions in q ) g (q d ) . = g (q d q Kpq = 0 IRn is the unique solution. If Kp > > 0 (suciently ), then q
R. Kelly, V. Santiba nez and A. Loria 207
Example 8.1
Consider the model of an ideal pendulum Jq + mgl sin(q ) =
l g m
where g (q ) = mgl sin(q ). The equilibria equation takes the form: = mgl [sin(qd q ) sin(qd)] . kpq Consider the following numeric values, J =1 mgl = 1 kp = 0.25 qd = /2 . Either via a graphical method or numeric algorithms, one may verify the equilibria, q 0 0.51 4.57 , , . q 0 0 0
R. Kelly, V. Santiba nez and A. Loria 209
Consider now a larger value for kp (suciently large), e.g., kp = 1.25 In this scenario the origin is the unique equilibrium, i.e., q 0 = IR2 . q 0
210
) along the trajectories of the closed loop The time derivative of V ( q, q system results
Q
( ) = q T Kv q . V q, q By invoking Lemma 2.2, we conclude that (t) and q (t) are bounded q (t), is square integrable, that is, q
213
( t) q
min{Kp}
(23)
(24)
214
Example 8.2
Consider again the model of the ideal pendulum Jq + mgl sin(q ) = ,
l g m
The potential energy function is U (q ) = mgl[1 cos(q )] and kU = 0. Consider the numeric values J =1 mgl = 1 kp = 0.25 kv = 0.50 qd = /2 . Assume that we use the PD control with desired gravity compensation to control it from the initial conditions q (0) = 0 and q (0) = 0. It is easy to verify that g (qd) = mgl sin(/2) = 1 1 1 2 kpq (0) + mglq (0) + (mgl)2 = 3.87 . V ( q (0), q (0)) = 2 2kp
R. Kelly, V. Santiba nez and A. Loria 216
According to the bounds (23) and (24), we get mgl + 2 q ( t) [mgl + kpq (0)] + (mgl)2 2 117.79 [ rad ] kp
2
35 30 25 20 15 10 5 0
q (t)2 [rad2 ]
..... .. .. .. . . . . . . . . . ........... .. . ... ...... .......................................................................... . . ......... . . . . . .. . . .. . . ........ .............. ..............
10
20
30 t [sec]
and
q 2 ( t)
rad sec
3 2 1 0
10
20
30 t [sec]
The solutions tend precisely to one among the three equilibria which do not correspond to the origin. PD control with desired gravity compensation may fail to verify the setpoint control objective.
219
Unicity of equilibrium
For robots having only revolute joints and considering the closedloop equation (22), we have. The equilibria satisfy q
T
T T
= q
T T
IR2n where
If min{Kp} > kg we have (by contraction mapping theorem) T q T the unique equilibrium is the origin, q
R. Kelly, V. Santiba nez and A. Loria
= 0T 0T
IR2n.
220
) is radially unbounded and positive denite because M (q ) > 0 V ( q, q and min{Kp} > kg .
R. Kelly, V. Santiba nez and A. Loria 221
( ) results in V ) = q T Kv q 0 The time derivative of V ( q, q q, q By applying the La Salles Theorem 2.7, we have The set is given by = = (x ) = 0 x IR2n : V q x= q ( ) = 0 q, q IR2n : V
IRn, q = 0 IRn} . = {q (0)T q (0)T q = 0 IR2n is the unique initial condition in for which x(t) for all t 0. This is enough to guarantee global asymptotic stability of the origin T T q T = 0 IR2n. q
R. Kelly, V. Santiba nez and A. Loria 222
Example 8.3
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Figure 60: Diagram of the Pelican prototype robot with 2 degrees of freedom.
R. Kelly, V. Santiba nez and A. Loria 223
whose components of the gravitational torques vector g (q ) are given by g1(q ) = [m1lc1 + m2l1]g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) .
i,j,q
224
Consider the PD controller with desired gravity compensation where min{Kp} > kg . In particular, we pick Kp = diag{kp} = diag{30} [Nm/rad] , Kv = diag{kv } = diag{7, 3} [Nm sec/rad] .
The components of the control input are given by 1 kv q 1 + g1(q d) , 1 = kpq 2 kv q 2 + g2(q d) . 2 = kpq
225
The initial conditions corresponding to the positions and velocities, are set to q1(0) = 0, q2(0) = 0 , 2(0) = 0 . q 1(0) = 0, q The desired joint positions are chosen as qd1 = /10 [rad] qd2 = /30 [rad] , hence, the initial state is set to 0.3141 /10 (0) q /30 0.1047 [ rad ] . = = 0 0 (0) q 0 0
R. Kelly, V. Santiba nez and A. Loria 226
[rad]
.... ... q ... 1 ... ... ... .. ..... q 2 .............. .... .......... .....................................................................................................0.0359 . .. . .. . .. . . . . .. . .. . .. . .. . . . .................................................................................................................. . .. . . . .. . .. . . . . . . .. . .. . . ..... ..... ..... . ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... .... . . .. .. . . . .. .. .. . . . 0.0138
0.1
0.0 0.5 1.0 1.5 2.0 t [sec]
Figure 61: Position errors q 1 and q 2 The position errors do not vanish due to nonmodeled friction eects.
227
We show now a global asymptotic stability analysis without using La Salles theorem Consider the following Lyapunov function candidate, T 0 2 M ( q ) K q q 2 p 1+ q 1 2 1+ 0q M (q ) q q M (q ) 1 T + q Kpq , + U (q ) U (q d ) + g (q d ) q 1 ) f (q
T
R. Kelly, V. Santiba nez and A. Loria 228
) = V ( q, q
where 0 > 0, 1 > 2 and 2 > 2 are chosen so that 2min{Kp} > 1 > 2 kg 2 = 21 >2 1 2
with =
0 1+ q
230
The time derivative of the above Lyapunov function candidate takes the form ( q TKpq ) = q TKv q + q TM (q )q + q TKv q V q, q ) T [g (q d) g (q )] TM (q )q , TC (q , q q q q q which is upperbounded by q q T
Q
( ) V q, q
1 2 Max {Kv }
1 20 min {Kv } 2
q q
1 [min{Kv } 20(kC1 + 2 )] q 2
231
The matrix Q is positive denite if it holds that min{Kp} > kg , 2min{Kv }(min{Kp} kg ) 2 Max {Kv } and we have that > 0 if min{Kv } > 0 . 2[kC1 + 2 ] > 0 ,
232
( ) V q, q
0 min{Q} 1+ q
+ q
2
q 2
2 q 0min{Q} q 1+ q 2 which is a negative denite function. T q T We conclude that the origin q asymptotically stable (Theorem 2.4).
T
= 0 IR2n is a globally
233
Kv d q qd
Kp
Ki
t
0
Figure 62: PID control. + Ki + Kv q The PID control law is given by = Kpq
t ( ) q 0
d , where
234
The control law may be expressed via the two following equations, + Ki + Kv q = Kpq = q . The closed loop equation is q d q = q dt + Ki C (q , q d M (q )1 Kpq )q g (q ) + Kv q q q
235
If the desired position q d(t) is constant, the equilibrium is 1 Ki g (q d) q IR3n . = 0 q 0 This equilibrium may be translated to the origin (via change of variable) z = Ki1g (q d) .
236
The corresponding closed loop equation is z q d . = q q dt )q g (q )] Kv q + Kiz + g (q d) C (q , q M (q )1 [Kpq q (25) Above equation is autonomous and its unique equilibrium is the origin T T q T = 0 IR3n. zT q
237
For the sequel,we adopt the following global change of variables, w I q = 0 q 0 I I 0 0 z with > 0. 0q I q
238
. (26)
239
If Kp and Kv are suciently large and Ki suciently small in the following sense min{M }min{Kv } Max{Ki} , > 2 { K } k Max {M } min p g and moreover min{Kp} > kg , then, the setpoint control objective is achieved locally.
R. Kelly, V. Santiba nez and A. Loria 240
, w) = V ( q, q
0 0
w 0 0 M (q ) q Kv M (q ) M (q ) q
1 1 T ) U (q d ) + q T g (q d ) + U (q d q Kp Ki q + q 2 U (q ) denotes as usual, the robots potential energy, and is a positive constant satisfying Max{Ki} min{M }min{Kv } > > . 2 min{Kp} kg Max {M }
R. Kelly, V. Santiba nez and A. Loria 241
Q11 0
0 ) Q22(q
q q
242
<
( , w) is negative semidenite. on which V q, q Therefore, according with Theorem 2.2, the origin of the closed loop equation (26), is a stable equilibrium.
243
Asymptotic stability
We may use La Salles theorem (Theorem 2.7). The set is given by = (x ) = 0 x IR3n : V w ( , w) = 0 IR3n : V q, q = x=q q = 0 IRn, q = 0 IRn} . = {w IRn, q (0)T q (0)T w(0)T q = 0 IR3n is the only initial condition in for which x(t) for all t 0. We conclude that the origin is locally asymptotically stable.
R. Kelly, V. Santiba nez and A. Loria 244
Tuning procedure
The preceding stability analysis allows to extract a simple tuning procedure. Max{Ki} min{Ki} > 0 Max{Kp} min{Kp} > kg Max{Ki} Max2{M } . Max{Kv } min{Kv } > min{Kp} kg min{M } It requires the knowledge of the structure of M (q ) and g (q ). Nonetheless, it is sucient to have upper bounds on Max{M (q )} and kg , and a lower bound for min{M (q )}.
245
Example 9.2
Consider the 2DOF prototype robot showed in Figure 63.
y g
Link 1
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
246
The components of the gravitational torques vector g (q ), are given by g1(q ) = (m1lc1 + m2l1)g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2).
247
Firstly, we compute the value of kg using the numeric values listed in Table 5.1 kg = n Max
i,j,q
gi(q ) qj
We proceed now to compute numerically min{M (q )} and Max{M (q )} min{M (q )} = 0.011 Max{M (q )} = 0.361, which correspond to q2 = 0.
R. Kelly, V. Santiba nez and A. Loria 248
kg m2 , kg m2
By following the tuning procedure, we nally determine the following matrices, Ki = diag{1.5} [Nm / (rad sec)] , Kp = diag{30} [Nm / rad] , Kv = diag{7, 3} [Nm sec / rad] .
249
[rad]
. . . . . . . . . . . . . . . . . . q ........... .....1 .......................... . . . ......................................... . . .................................................. . . q ........... 2 . .. . . . .. .. . . . . . .. . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... . . . .... .. .. . .... . . . .. . .. .. .. .. . . ..... ..... ..... ..... ..... ..... .... .. . . .. . .. . .. .............................................................................. . . .. . . .. . . . .
0.1
0.0 12.5 25.0 37.5 50.0 t [sec]
Figure 64: Position errors q 1 and q 2 From Figure we may conclude that the transient response is slower This is due to the fact that the tuning procedure limits min{Ki} by a relatively small upperbound.
R. Kelly, V. Santiba nez and A. Loria 250
[rad]
..... ... ... q ... 1 ... ... .... ..... q 2 ........... ..... ......... ....... ........................... . . . . . . . . . . ..... ..... ..... ........ . . . .. . . .. .. .... . .................. . . .. . . . . ...... . . . . . . . ..... . . . .... ............................................................... . ..... . . . . . . ..... . . . .... . . . .. . . . . ...... .
0.1
0.0 0.5 1.0 1.5 2.0 t [sec]
Figure 65: Position errors q 1 and q 2 If the tuning procedure is violated the performance of the PID controller improves up. The latter results have been obtained increasing the values of Ki to Ki = diag{70, 100} [Nm / (rad sec)] .
R. Kelly, V. Santiba nez and A. Loria 251
252
Introduction
Introduction
gives
q q d = dt )q g (q )] q M (q )1 [ (t) C (q , q
253
Introduction
where M (q ) IRnn is the inertia matrix, )q IRn is the vector of centrifugal and Coriolis forces, C (q , q g (q ) IRn is el vector of gravitational forces and torques and IRn is a vector of external forces and torques applied at the joints. ,q IRn denote the position, velocity and joint The vectors q , q acceleration respectively.
254
Introduction
d and q d referred to as Given a set of vectorial bounded functions q d, q desired joint positions, velocities and accelerations. The objective of tracking control consists on nding such that
t
( t) = 0 lim q
where IRn stands for the joint position errors vector q (t) := q d(t) q (t) , q ( t) = q (t) stands for the velocity error. d ( t) q q
R. Kelly, V. Santiba nez and A. Loria 255
Introduction
In general, a control law may be expressed as d, q d , M (q ), C (q , q ), g (q )) . ,q , q d, q = (q , q For practical purposes it is desirable that the controller does not depend on . the joint acceleration q
qd d q d q
CONTROLLER
ROBOT
q q
256
Introduction
A methodology to analyze the stability may be summarized in: 1. Derivation of the closed loop dynamic equation. 2. Representation of the closed loop equation in the statespace form, d qd q d q dt q
qd d q d q
d, q d , M (q ), C (q , q ), g (q )) . , q d, q = f (q , q
CONTROLLER + ROBOT
q q
control
closedloop
system:
Inputoutput
257
Introduction
3. Study of the existence and possible unicity of the equilibrium 4. Proposition of a Lyapunov function candidate to study the stability of any equilibrium of interest 5. Alternatively to step 4), determine the qualitative behavior of the solutions of the closed loop equation.
258
Introduction
The controllers that we consider are, in order, Computed torque control and Computed torque+ control. PD control with compensation and PD+ control. Feedforward control and PD plus feedforward control.
259
In this chapter we study two controllers that do not present explicitly the linear PD term Computedtorque control, Computedtorque+ control.
260
Computedtorque control
g (q ) d q M (q ) ROBOT q q
Kv d q qd
Kp
) C (q , q
Figure 68: Computedtorque control. The corresponding equation to computedtorque control is given by + Kpq + C (q , q )q + g (q ) , d + Kv q = M (q ) q where Kp and Kv are symmetric positive denite matrices.
R. Kelly, V. Santiba nez and A. Loria 261
(27)
The closed loop equation is + Kpq . d + Kv q M (q ) q = M (q ) q Above equation reduces to + Kpq =0 + Kv q q which in turn, may be expressed in terms of the state vector q q q 0 d = = dt Kv q q Kpq Kp where I is the identity matrix of dimension n.
R. Kelly, V. Santiba nez and A. Loria 262
T q
as
q I , Kv q
(28)
It is important to remark that the closed loop equation (28) is represented by a linear autonomous dierential equation, whose unique equilibrium point is given by q
T
T T
= 0 IR2n.
263
I q I q
1 T + q + q Kp + Kv 2I q (29) q 2
(30)
265
q In view of Theorem 2.4, we get that the origin q globally uniformly asymptotically stable and therefore
T t
T T
= 0 IR2n is
( t) = 0 lim q
( t) = 0 . lim q
Kv
266
Example 10.2
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Consider the Computedtorque control (27) on this robot for tracking control. The desired reference trajectory, q d(t), is given by Equation (13).
2.0 1.5 1.0 0.5 0.0
[rad]
.. d2 .. .. .. ... ... ... ... ... .... ... .... . . . . . . . . . .. . . . . . . . .. . . . . . . . . . . . . q d1 . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ...... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . .. .. .. .. . . . . . . . .. . . . .. . . . .. ... . . .. . . .. ... . . ..... . . . . . . . . . . . . . . .. . . . . . . . . ..... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ...... ..... ...... ... ...... ..... .. ...... .. ............ ... . .. .. .. . . ............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
10 t [sec]
q d1 q d2
b1[1 e
2.0 t3
3
] + c1[1 e
2.0 t3
3
] sin(1t) ] sin(2t)
[rad]
b2[1 e2.0
] + c2[1 e2.0
where b1 = /4 [rad], c1 = /9 [rad] and 1 = 4 [rad/sec], and b2 = /3 [rad], c2 = /6 [rad] and 2 = 3 [rad/sec]. d(t), were analytically found, and they correspond to d(t) and q q Equations (14) and (15), respectively.
269
Kv
1 / sec2 ,
where we used 1 = 38.7 [rad / sec] and 2 = 118.3 [rad / sec]. The initial conditions are chosen as q1(0) = 0, q2(0) = 0 q 1(0) = 0, q 2(0) = 0 .
270
[rad] q 1 . ........ . ................... ...... ...... . . ........... ............ ............. . ............. ... . . ............. . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . .. . . ........ ...... . .. .... . .. . . .... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ................ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ....... .. . .. .... . . ....... . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . ....... .......... .......... ...... q 2
0.01 0.02
10 t [sec]
Figure 71: Position errors. The steady state position errors are not zero due to the friction eects of the actual robot which nevertheless, are neglected in the analysis.
271
Computedtorque+ control
g (q ) M (q ) ROBOT q q
d q
1 p+
) C (q , q
Kv Kp p p+ d q qd
The equation corresponding to the computedtorque+ controller is given by + Kpq + C (q , q )q + g (q ) C (q , q ) d + Kv q = M (q ) q where Kv and Kp are symmetric positive denite design matrices, , and velocity q IRn is obtained by ltering the errors of position q that is, b bp + Kpq , Kv q (32) q = p+ p+ p is the dierential operator (i.e., p := , b are positive design constants.
R. Kelly, V. Santiba nez and A. Loria
(31)
d dt )
273
The computedtorque+ control law is dynamic The expression (32) in the state space form is a linear autonomous system given by q I 0 1 Kp Kv d 1 + = dt 2 2 0 I q 0 I q 1 = [ I I ] [ 0 I ] 2 q where 1, 2 IRn are the new state variables.
(33)
(34)
274
(35)
275
The study of global asymptotic stability of the origin of the closed loop equation (35) is actually an open problem. Nevertheless, using Lemma 2.2 and Corollary A.2 we will show that (t) and (t) are bounded, and ( t) , q the functions q that the motion control objective is veried.
276
Toward this end, we can get an equivalent closed loop equation + ] + C (q , q ) = 0 . M (q ) [ Consider now the following nonnegative function 1 ) 0 ) = TM (q d q V (t, , q 2 P ) is given by The derivative with respect to time of V ( , q ( , q ) = TM (q ) 0 V Q
277
We conclude
n L Ln 2 and
(t)T (t)
(0)) 2t 2V ( (0), q e .
( t) = 0 , lim q
278
Example 10.3
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Consider the Computedtorque+ control described by (31), (33) and (34) applied to this robot. d(t) and q d(t) are those used in the previous example. q d ( t) , q
2.0 1.5 1.0 0.5 0.0
[rad]
d2 ...... ........ ......... ......... . . .. .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . q d1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . ....... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... . . . . . . . . . . . . . . . . . . .. . .. . .. . .. . .. . .. . . . . . . . . . .. ... . . .. .. ... . .. .. ... . ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... . ....... . .. . ...... . . ... .. ....... . . . . . . . ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . .. .. . . . . ... ..... ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
10 t [sec]
Kv
1 / sec2
= 60 . The initial conditions of the controller state variables are xed at 1(0) = 0, 2(0) = 0 . The initial conditions of the actual positions and velocities are set to q1(0) = 0, q2(0) = 0 q 1(0) = 0, q 2(0) = 0 .
R. Kelly, V. Santiba nez and A. Loria 281
[rad] q 1 . ......... .......... ..... .......... .......... . . ......... . . . ...... . . . . ....... .. .. ............ . .............. . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ........... .. ...... .. . .. .......... . ....... q 2
0.01 0.02
10 t [sec]
Figure 75: Position errors. It is interesting to remark that the plots obtained with the Computedtorque control, present a considerable similarity to those of gure 75.
282
We present two controllers whose control laws are based on the dynamic equations of the system but which also involve certain nonlinearities that are evaluated along the desired trajectories. PD control with compensation PD+ control.
283
) Kv C (q , q
Kp
d q qd
Figure 76: PD control with compensation. The PD control law with compensation may be written as + C (q , q + M (q ) q ) [q d + + Kv q d + q = Kpq q ] + g (q ),
T T > 0, Kv = Kv > 0 IRnn and = Kv 1Kp . where Kp = Kp
R. Kelly, V. Santiba nez and A. Loria 284
(36)
The equation of closed loop is , + C (q , q + Kv q + q ) q M (q ) q q = Kpq which may be expressed in terms of the state vector q
T
T T
as
T T
285
The stability analysis may be carried out by considering Lyapunov function candidate T T T ) M (q d q ) q q 2Kp + M (q d q 1 ) = . , q V (t, q 2 ) ) M (q d q q M (q d q q We rewrite it in the following form
T 1 + ) = q , q + TKpq V (t, q q M (q ) q q +q 2
(37)
286
I T 0 I
Kp 0 0 M (q ) B T AB
I 0 I
q q
It is positive denite (because Kp > 0, Kv > 0 and hence B T AB > 0) and radially unbounded.
287
Correspondingly, since the inertia matrix is bounded uniformly in q , we have that 1 + , q ) Max{M } q q V (t, q 2 ) is also decrescent. , q hence, V (t, q
2
+ Max{Kp} q
288
T q , q ) = q Kv q TTKv V (t, q q T T Kv 0 q q . = 0 Kv q q
Globally negative denite function. From Theorem 2.4 we conclude immediately global uniform asymptotic T T T q stability of the equilibrium q = 0 IR2n.
289
Example 11.1
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Consider this robot under PD control with compensation (36). d(t) and q d ( t) It is desired that the robot tracks the trajectories q d(t), q represented by Equations (13)(15).
2.0 1.5 1.0 0.5 0.0
[rad]
d2 ......... ......... ......... ........ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . q d1 . . . . . . . . . . . . . . . . . . . . . . . . . .... . ...... . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . ..... . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . .. . .. . . . . . . . .. ... . . . .. . . . . . .. ... . .. . ..... . . . . . . . . . . . ..... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... .......... ....... ..... ................. ....... ..... ....... . .. . . . . .. . .. . . . . ............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
10 t [sec]
291
q d1 q d2
b1[1 e
2.0 t3
3
] + c1[1 e
2.0 t3
3
] sin(1t) ] sin(2t)
[rad]
b2[1 e2.0
] + c2[1 e2.0
where b1 = /4 [rad], c1 = /9 [rad] and 1 = 4 [rad/sec], and b2 = /3 [rad], c2 = /6 [rad] and 2 = 3 [rad/sec]. d(t), were analytically found, and they correspond to d(t) and q q Equations (14) and (15), respectively.
292
The symmetric positive denite matrices Kp and Kv are chosen so that Kp = diag{200, 150} [N m / rad] , Kv = diag{3} [N m sec / rad] ,
The initial conditions corresponding a the positions and velocities are q1(0) = 0, q2(0) = 0 2(0) = 0 . q 1(0) = 0, q
293
[rad]
1 ....... .. ..... ........ ..... ...... . ........ .... . . . . ......... .......... . . . .... . . . ... .. . .. ... ... ........ ..... . . . . . .. ....... ......... . . . . . . . . .. ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... ..... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ........ . . . . . . . . . . . . . . . . . . .... .... . . .... ... . . ........ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ......... .......... ......... ......... ... q 2
0.01 0.02
10 t [sec]
Figure 79: Position errors. (t), by virtue of The experimental steady state tracking position errors q friction phenomena in the actual robot, are not zero.
294
PD+ control
g (q ) d q M (q ) ROBOT q q
) C (q , q d q qd
Kv
Kp
Figure 80: PD+ control. The control law of PD+ control is given by + M (q ) )q d + g (q ) + Kv q = Kpq q d + C (q , q where Kp, Kv IRnn are symmetric positive denite matrices
R. Kelly, V. Santiba nez and A. Loria 295
(38)
The closed loop equation may be written as q q d . = dt )q C (q d q )1 Kpq , q d q Kv q M (q d q q Nonlinear nonautonomous dierential equation The only equilibrium point is the origin q
T
T T
= 0 IR2n.
296
To analyze the stability of the origin consider now Lyapunov function candidate T K q 1 p 2 0 q 0 q ) M (q d q q (39) P
) = , q V (t, q
1 T + 1q , M (q )q TKpq q 2 2
297
(t, q ) = q Kv q , q V T 0 q = 0 q
0 q 0 Kv q
From Theorem 2.3 we conclude stability of the origin. La Salles theorem cannot be used to conclude global asymptotic stability. Alternatively, we may use Lemma 2.2 position and velocity errors are bounded and the velocity error is squareintegrable (
0
( t) q
dt < . )
298
Example 11.3
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Consider the application of the PD control+ (38) on this robot. The joint desired trajectories of position, velocity and acceleration: q d(t), d(t), are given by Equations (13)(15). d(t) and q q
2.0 1.5 1.0 0.5 0.0
[rad] q d2 ....... ....... ....... ........ . . . . .. . .. . .. . . . .. . . . . . . . . . . . . . . . . . . . . . . q d1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. .. . . . . . . . . . . . ...... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . .. . ... .. .. .. . . . . . ... . . . . . . .. ... . . . .. ... . .. ..... . .. ... . .. . . . . . . . . . . . . . . . . . . .. . . ..... ... . . . . . . . . . . . . . . . . . . . . . .. . .. .. . . . . . . . . . ....... ....... .... .. ..... . . . . . . . . . . . . . . . . . . .... ... ... ... ... . . . . . . . . .. . .. . . . ............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
0 2 4 6 8 10 t [sec]
300
The symmetric positive denite matrices Kp and Kv are chosen as Kp = diag{200, 150} [N m / rad] Kv = diag{3} [N m sec / rad] .
The initial conditions corresponding to the positions and velocities, are xed as q1(0) = 0, q2(0) = 0 2(0) = 0 . q 1(0) = 0, q
301
[rad] q . 1 . .. .. .. . . . . . . . .. . . . . .. . . .. . .. .. . . .. . . . . . . . . . . . ..... . . . . ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... ..... .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ............ .... . .......... . ... .. . . ............. . . .. . .. . .... . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . q . . . . . . . . 2 . . . .
0.01 0.02
10 t [sec]
Figure 83: Position errors. (t), by virtue of The experimental steady state tracking position errors q friction phenomena (neglected in the analysis) in the actual robot, are not zero.
302
M (q )
) = , q V (t, q
M (q )
q (40) q
) + , q = W (t, q
0 T M (q )q q 1+ q ( q)
The positive constant 0 is chosen so as to satisfy simultaneously min{Kp} > 0 > 0 Max{M (q )} min{Kv } > 0 > 0 2 (kC1 + 2Max{M (q )}) 2min{Kp}min{Kv } 2 > 0 > 0 d Max) (Max{Kv } + kC1 q This, implies that the matrix Kp 2M (q ) > 0 The function (40) may be rewritten as
T 1 1 T ) = q + q + q , q M (q ) q Kp 2M (q ) q + q V (t, q 2 2
q q
, (41)
The matrix on the right hand side of inequality (41) is positive denite in view of the condition on 0, Max{Kp} > 0 > 0 Max{M (q )} q IRn
Thus, the function (40) is positive denite, radially unbounded and decrescent.
R. Kelly, V. Santiba nez and A. Loria 305
( ) = q + ( + ( TKv q TM (q )q T C (q , q ) V q, q q q )q q )q ) a( q, q + . + Kv q ( q ) q T Kpq ( q ) q TM (q )q ) b( q, q
306
q q
q q
(t, q ) , q V
307
where the symmetric matrix Q is given by min{Kp} 1 2 (Max {Kv } + kC1 q d ) 1 2 d ) (Max{Kv } + kC1 q
1 20 min {Kv }
Q=
=0
308
Feedforward control
d q M (q d ) ROBOT q q
d) C (q d , q
g (q d )
d q qd
310
The behavior of the control system is described by q q d , = dt + gd g] d Cq q d + Cd q q M 1 [(Md M ) where ), C d = C (q d , q d) M = M (q ), Md = M (q d), C = C (q , q g = g (q ) and g d = g (q d). T = 0 IR2n is an equilibrium point of the previous q The origin q equation but in general, it is not the only one.
T
R. Kelly, V. Santiba nez and A. Loria 311
Example 12.3
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Consider the application of feedforward control (42) on this robot. The desired trajectory is given by q d(t) which is dened in (13).
2.0 1.5 1.0 0.5 0.0
[rad] q d2 ... ... ... ........ . ... ... ... ... ... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . q d1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ...... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . .. .. . . . . . . .. . ... . . . . . .. . .. . . .. ... . . . .. . . . .. ... . ..... . . . . . . . . . . .. . .. . . . . . . . ..... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. ...... .... ...... .. ...... . ..... .. ............ ... .... . .. . .. . . . ............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
0 2 4 6 8 10 t [sec]
Figure 86: Desired reference trajectories. The initial conditions are chosen as q1(0) = 0, q2(0) = 0 q 1(0) = 0, q 2(0) = 0 .
R. Kelly, V. Santiba nez and A. Loria 313
[rad]
...... ........... ........... . . ... .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... . . . . . . . . . . . . . . . . . . . . . ... . . . . . . . . . . . ... . . . . . . . . . .... .. . . . . . . . . . ... . . . . . . . . . . . . . . . . . . ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . q . . . . . . . . . . . .. .. 1 .... ... . . . . . ......... . . . . . . . . . . . . . . . . . . . . ....... . . ..... ............ .. . . . ..... .... ...... ...... . ............ ..... .. . ..... ... .. ..... ... ... ..... . ... . . ..... .... .. ... ... ........ ... ..... . .. . ... . ... . . ........ ........ ... ... . ..... .... ....
q 2..............
0.5
10 t [sec]
Figure 87: Position errors (t) tend to an oscillatory behavior. Figure shows the position errors q Naturally, this behavior is far from satisfactory. A rigorous generic analysis of stability or instability seems to be an impossible task.
R. Kelly, V. Santiba nez and A. Loria 314
d) C (q d , q
g (q d )
Kv
Kp
d q qd
315
The closed loop equation may be written as: q q d , = dt C (q , q h( ) )q Kv q M (q )1 Kpq q, q q where the socalled residual dynamics, is given by ) = [M (q d) M (q )] d ) C (q , q )]q d + g (q d ) g (q ) . h( q, q q d + [C (q d, q The origin [q
T T q ]T = 0 IR2n of the state space is an equilibrium.
(43)
k (x ) k (y )
1 Max Kp
R. Kelly, V. Santiba nez and A. Loria
h(x, 0) h(y , 0) .
317
d h(x, 0) h(y , 0) kg + kM q
d M + kC2 q
2 M
xy .
318
xy .
Invoking the contraction mapping theorem, we conclude that d min {Kp} > kg + kM q d M + kC2 q
2 M
It is a sucient condition for k( q ) to have a unique xed point, and therefore, for the origin of the state space to be the unique equilibrium of the system in closed loop (43).
319
We assume that given a constant > 0, Kv is chosen suciently large in the sense that Max{Kv } min{Kv } > kh1 + b , and so is Kp but in the sense that Max{Kp} min{Kp} > 3 [2 a + kh2] + kh2 4 [min{Kv } kh1 b]
2
320
so that
(44)
where kh1 and kh2 are dened in Chapter 4 while the constants a and b are given by 1 d M + kh1] , a = [Max{Kv } + kC1 q 2 b = 4 Max{M } + 2 kC1 .
321
Lyapunov function candidate. To carry out the stability analysis, consider Lyapunov function candidate + 1q ) = 1q T M (q )q , q + tanh( T Kpq V (t, q q )T M (q )q 2 2 where > 0 is a given constant and tanh(x1) . . tanh(x) = tanh(xn) with x IRn.
R. Kelly, V. Santiba nez and A. Loria 322
(45)
The Lyapunov function candidate (45) satises the following inequality ) 1 , q V (t, q 2 q q
T
q q
It is positive denite and radially unbounded since Kp is positive denite min{Kp} > 0, and it is chosen so as to satisfy (44). Max{Kp} min{Kp} >
2 2 1
2 Max {M } . min{M }
323
Max{Kp} 1 Max{M }
1 Max{M } Max{M }
q q
whose right hand side is positive denite and radially unbounded since the condition 2 Max{Kp} > 21 Max{M }, is satised under hypothesis (44) on Kp. ) is decrescent. , q This means that V (t, q
324
Time derivative. The time derivative of the Lyapunov function candidate yields
T T tanh( Kv q , q ) = q q Sech2( V (t, q q )T M (q )q q )T Kpq + tanh( )T q tanh( q )T Kv q q )T C (q , q T ) tanh( ). h( q, q q )T h( q, q q
325
which can be upperbounded by (t, q , q ) V min {Kp } T kh2 tanh( q) 3 1 kh2 q a 2 where a =
tanh( q) q
+ kh1] ,
R( ) is positive denite (t, q ) is globally negative denite. , q therefore, V Theorem 2.4 concludes global uniform asymptotic stability of the origin.
R. Kelly, V. Santiba nez and A. Loria 326
Tuning procedure. It can be summarized as Derivation of the dynamic robot model to be controlled. Particularly, ) and g (q ) in closed form. computation of M (q ), C (q , q Computation of the constants Max{M (q )}, min{M (q )}, kM , kM ,kC1 , kC2 , k and kg . For this, it is suggested to use the information given in Table 4.1. d Computation of q to the robot.
Max,
d q
Max
327
Computation of the constants s1 and s2 given by d s1 = kg + kM q and d s2 = 2 k + kM q Computation of kh1 and kh2 given by d M, kh1 kC1 q s2 kh2 . 2 tanh s s1
R. Kelly, V. Santiba nez and A. Loria 328
d M + kC2 q
2 M
d M + kC1 q
2 M
Computation of the constants a and b given by a = 1 d [Max{Kv } + kC1 q 2 b = 4 Max{M } + 2 kC1 , + kh1] ,
where 2 =
n, 4 = 1.
329
Select > 0 and determine the design matrices Kp and Kv so that their smallest eigenvalues satisfy min{Kv } > kh1 + b, [2 a + kh2] + kh2 , min{Kp} > 3 4 [min{Kv } kh1 b] 2 2 2 1 Max {M } , min{Kp} > min{M } with 1 = 1, 3 = 1.
2
330
Example 12.5
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
332
) are given The elements of the centrifugal and Coriolis forces matrix C (q , q by ) = m2l1lc2 sin(q2)q C11(q , q 2 ) = m2l1lc2 sin(q2) (q C12(q , q 1 + q 2) ) = m2l1lc2 sin(q2)q 1 C21(q , q ) = 0. C22(q , q The elements of the vector of gravitational torques g (q ) are g1(q ) = (m1lc1 + m2l1)g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) .
333
Using the numeric values of the constants given in Table 5.1 as well as the formulas on Table 4.1, we get kM kC1 kC2 kg kM = 0.0974 kg m2 , = 0.0487 kg m2 , = 0.0974 kg m2 , = 23.94 kg m2/sec2 , = Max{M (q )} = 0.3614 kg m2 ,
= 9.52 [ rad/sec2] .
334
Using this information and the denitions of the constants from the tuning procedure, we get that s1 = 25.385 [N m] , s2 = 22.733 [N m] , kh1 = 0.114 kg m2/sec , kh2 = 31.834 [N m] , a = 1.614 kg m2/sec , b = 0.43 kg m2 .
The initial conditions corresponding to the positions and velocities, are chosen as q1(0) = 0, q2(0) = 0 2(0) = 0 . q 1(0) = 0, q
336
[rad] q .1 . . . . . . . . . . . . .. . . . . . .. . . . .. . . . . . .. . . . . . . . . . . ...... . . . . . . . .. . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . .. .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... ..... .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... . . . . .... . . .. ......... . .. .. .. . ......... . .... . .... .. .. ... . .. . . . . . .. . .. . . . . . . . . . . . . . . . . . . . . . . . . . q . . . . . . . . 2 . . . . . . . . .
0.01 0.02
10 t [sec]
Figure 90: Position errors In contrast to example 12.3 where the controller did not carry the PD term, the behavior abtained here is satisfactory.
337
338
Introduction to Part IV
Introduction to Part IV
We deal with a variety of topics: Control without velocity measurements Control under model uncertainty. Specically: PD control with gravity compensation and PD control with desired gravity compensation Introduction to adaptive robot control PD control with adaptive gravity compensation PD control with adaptive compensation.
R. Kelly, V. Santiba nez and A. Loria 339
Ch. 13. PD Control with gravity compensation and PD Control with precalculated gravity compensation
The interest to count on controllers without measurement of velocity, is twofold: no poor quality for certain bands of operation suppression of velocity sensors (no tachometers and resolvers) reduction in the production cost robot lighter.
340
The design of controllers that do not require velocity measurements to control robot manipulators is a topic of investigation broached in the decade of the 1990s to date, many questions remain open. the common idea: to propose state observers to estimate the velocity. In this chapter we present an alternative: , by the ltering of q substituting q through a rst order system of zero relative degree, whose output is denoted, by .
R. Kelly, V. Santiba nez and A. Loria 341
A statespace representation of above Equation is: = Ax AB q x = x + Bq where, x IRn represents the state vector of the lters, A = diag{ai} and B = diag{bi}.
R. Kelly, V. Santiba nez and A. Loria 342
In this chapter we present the study of the proposed modication for the following controllers: PD control with gravity compensation PD control with desired gravity compensation. the derivative part of both controllers is no longer proportional to the derivative of the position error q this motivates the quotes in D in the name of the controller.
343
= Ax AB q x = x + Bq where Kp, Kv IRnn are diagonal positive denite matrices, A = diag{ai} and B = diag{bi} and ai and bi are arbitrary real strictly positive constants for i = 1, 2, , n.
R. Kelly, V. Santiba nez and A. Loria 344
g (q ) Kv d q qd Kp ROBOT B
(pI + A)1AB
Figure 91: PD Control with gravity compensation. When q d is a constant vector the control law becomes Kv diag = Kpq bip q + g (q ). p + ai
345
it veries the setpoint control objective: limt q (t) = q d (with q d IRn constant) The closed loop equation may be rewritten as A + AB q d = q q dt )1 [Kpq ] C (q d q , q )q ] Kv [ B q M (q d q q which is an autonomous dierential equation = x + B q d. the origin
T
346
347
Time derivative of the Lyapunov function candidate 1 T T +q TKpq , q ) = q M (q ) M (q )q V ( , q q+ q 2 Bq . ]T Kv B 1 + [ B q Using the closed loop equation we obtain ( , q , q ) = [ B q ]T Kv B 1A [ B q ] V T 1 Kv B A Kv A 0 Kv A BKv A 0 = q 0 0 0 q ( , q , q ) is globally negative semidenite. V The origin is asymptotically stable (use the La Salles Theorem).
R. Kelly, V. Santiba nez and A. Loria 348
q q
Example 13.1
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
The components of the vector of gravitational torques g (q ) are given by g1(q ) = (m1lc1 + m2l1)g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) .
Consider the PD controller with gravity compensation with Kp = diag{kp} = diag{30} [Nm/rad] , Kv B = diag{kv } = diag{7, 3} [Nm sec/rad] , = diag{bi} = diag{30, 70} [1/sec] . A = diag{ai} = diag{30, 70} [1/sec] ,
350
The components of the control input are given by 1 kv 1 + g1(q ) 1 = kpq 2 kv 2 + g2(q ) 2 = kpq x 1 = a1x1 a1b1q1 x 2 = a2x2 a2b2q2 1 = x1 + b1q1 2 = x2 + b2q2 .
In terms of the state vector of the closed loop equation, the initial state is b1/10 9.423 (0) b2/30 7.329 /10 0.3141 q (0) = /30 = 0.1047 . 0 0 (0) q 0 0
R. Kelly, V. Santiba nez and A. Loria 352
Control of Robot Manipulators in Joint Space 0.4 0.3 0.2 0.1 0.0
[rad] ..... ... q ... 1 ... .... ...... ......... .... q ...... 2 ......................................................................................................... 0.0587 ......................... ................ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . .... . . .. .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... .... ..... . . .... . . . .. . . .. .. 0.0151
0.0 0.5 1.0 1.5 2.0 t [sec]
0.1
Figure 93: Position errors q 1 (t) and q 2 ( t) . Figure presents the experimental results, (t) tends asymptotically to a constant nonzero value (due to the q nonmodeled friction eects).
353
= Ax AB q x = x + Bq where Kp, Kv IRnn are diagonal positive denite matrices, A = diag{ai} and B = diag{bi} with ai and bi arbitrary real strictly positive constants for all i = 1, 2, , n.
R. Kelly, V. Santiba nez and A. Loria 354
g (q d ) q
Kv d q qd Kp
ROBOT B
(pI + A)1AB
Figure 94: PD Control with desired gravity compensation. When q d is a constant vector the control law may be expressed by bip Kv diag = Kpq q + g (q d ), p + ai
R. Kelly, V. Santiba nez and A. Loria 355
If min{Kp} > kg , then it veries the setpoint control objective that is, limt q (t) = q d The closed loop equation may rewritten as: A + AB q d q q = dt ]+ g(q d) C (q d q , q )q g (q d q )] Kv [ B q M (q )1 [Kpq q which is an autonomous dierential equation. = x + B q d. the origin
T
356
357
The time derivative of the Lyapunov function candidate yields 1 T T T , q ) = q M (q ) q ) + g (q d )T q g (q d q M (q )q V ( , q q+ q 2 Bq . + [ B q ]T Kv B 1 TKpq +q Using the closed loop equation we obtain ( , q , q ) = ( B q )T Kv B 1A ( B q ) V
( , q , q ) is a globally semidenite negative function. V The origin is global asymptotically stable (use La Salles Theorem).
358
Example 13.2
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
The components of the vector of gravitational torques g (q ) are given by g1(q ) = (m1lc1 + m2l1)g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) .
kg
= n
max
i,j,q
kg m2/sec
.
360
Consider the PD control with desired gravity compensation satisfying: min{Kp} > kg .
In particular, these matrices are taken to be Kp = diag{kp} = diag{30} [Nm/rad] , Kv B = diag{kv } = diag{7, 3} [Nm sec/rad] , = diag{bi} = diag{30, 70} [1/sec] . A = diag{ai} = diag{30, 70} [1/sec] ,
361
The components of the control input are given by 1 kv 1 + g1(q ) 1 = kpq 2 kv 2 + g2(q ) 2 = kpq x 1 = a1x1 a1b1q1 x 2 = a2x2 a2b2q2 1 = x1 + b1q1 2 = x2 + b2q2 .
In terms of the state vector of the closed loop equation, the initial state is b1/10 9.423 (0) b2/30 7.329 /10 0.3141 q (0) = /30 = 0.1047 . 0 0 (0) q 0 0
R. Kelly, V. Santiba nez and A. Loria 363
Control of Robot Manipulators in Joint Space 0.4 0.3 0.2 0.1 0.0
[rad] .... . . q 1 . . . . . . . . . . . ... .... ..... q ..... 2............................ .........................................................................................0.0368 ........ ....... . . . . . . .. . . . . . . .... . . .... . .... . .... . . . .... .. .. . . . . . . . . .. .... . .... . . . . . . .. .... . .. .. .... . . .... . . . .... .. . . . . . .... . . .. .. .. .. . . .. .. .. . .. . . . .. . . .. .. . . .. ..... ..... ......... ..... .. . .. . .. .. .. .. .. .. . .. . . . . ..... . . . .... . . .. . .. .. .. . . . . .... . . .. . .. .. .. . . .. . . . . .. ..... . . .. . . .. . .. . .. .. . . . . . ..... 0.0145
0.0 0.5 1.0 1.5 2.0 t [sec]
0.1
Figure 96: Position errors q 1 (t) and q 2 ( t) Figure shows the experimental results, (t) tends asymptotically to a constant nonzero value (due to the q friction eects in the actual robot).
364
In the subsequent chapters we describe and analyze two adaptive controllers for robots.
366
(46)
Example 14.1
Consider the example of an ideal pendulum Dynamic model + mgl sin(q ) = ml2q where its mass m is concentrated at the tip, at a distance l from its axis of rotation. We identify: , and g (q, ) = mgl sin(q ). M (q, ) = ml2q ml2 = (assuming that both parameters are unknown) mgl it is a nonlinear vectorial function of the physical parameters.
R. Kelly, V. Santiba nez and A. Loria 368
[ q
sin(q )]
ml2 mgl
=: (q, q ) . contains nonlinear terms of the state, and is the vector of dynamic parameters. Property known as parameterization.
R. Kelly, V. Santiba nez and A. Loria
linearity
in
the
parameters
or
linear
369
2.
nm Moreover, if q , u, v , w Ln . then (q , u, v , w ) L
370
where the nominal model or nominal part of the model is, (q , u, v , w) = M0(q )u + C0(q , w)v + g 0(q ) .
R. Kelly, V. Santiba nez and A. Loria 371
M0(q ), C0(q , w) and the vector g 0(q ) are the parts of ) and g (q ) M (q ), C (q , q do not depend on (unknown dynamic parameters). IRm, we have Given a vector = (q , u, v , w) )u + C (q , w , )v + g (q , ) M0(q )u C0(q , w)v g (q ). M (q , 0 A particular case is when u = v = w = 0 IRn. In this scenario g (q , ) = (q , 0, 0, 0) + g 0(q ) .
372
Example 14.7
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Dynamic model ) C12(q , q ) C11(q , q g 1 (q ) M11(q ) M12(q ) = q+ q+ ) C22(q , q ) M21(q ) M22(q ) C21(q , q g 2 (q ) ) M (q ) g (q ) C (q ,q where
2 2 2 l M11(q ) = m1lc + m + l 2 1 1 c2 + 2l1 lc2 cos(q2 ) + I1 + I2 2 M12(q ) = m2 lc 2 + l1 lc2 cos(q2 ) + I2 2 M21(q ) = m2 lc 2 + l1 lc2 cos(q2 ) + I2 2 M22(q ) = m2lc 2 + I2
) = m2l1lc2 sin(q2) [q C12(q , q 1 + q 2] ) = m2l1lc2 sin(q2)q 1 C21(q , q ) = 0 C22(q , q g1(q ) = [m1lc1 + m2l1] g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) . We have selected as parameters of interest, m2, I2 and lc2 We dene the vectors u=
R. Kelly, V. Santiba nez and A. Loria
u1 u2
v=
v1 v2
w=
w1 w2
.
375
0 0
0 0 0 0 m1lc1g sin(q1) 0 .
The control laws may be written in the functional form d, q d , M (q ), C (q , q ), g (q )) . , q d, q = (q , q Giving a little more structure we have , q d, q d, q d ) + M (q )u + C (q , w )v + g (q ), = 1 (q , q where
379
, q d, q d, q d), usually corresponds to linear control The rst term 1(q , q terms of PD type, i.e., , q d, q d, q d) = Kp[q d q ] + Kv [q d q ] 1 (q , q where Kp is the gain matrix of position Kv is the velocity (or derivative gain). It does not depend explicitly on the dynamic model and on q d, q d and q d. In the second term u, v , w IRn depend on q , q It depends explicitly on the dynamic model
380
In general an adaptive controller is formed by two main parts: Control law or controller. Adaptation law. Control law in the generic form )u + C (q , w , )v + g (q , ) , q d, q d, q d ) + M (q , = 1 (q , q
( t) =
0
(0) ,q , q d, q d, q d) ds + (s, q , q
where (0) IRm are design parameters = T IRmm and is usually diagonal and positive denite (adaptation gain) is a vectorial function to determine, of dimension m. (0) is an arbitrary vector in practice, we choose it as the best approximation available on .
382
qd d q d q
) ,q d, q d , (t, q , q
ROBOT
qd q
,q , qd, q d, q d ) ds (s, q , q
0
Figure 98: Adaptive control of robots: blockdiagram. An equivalent representation of the adaptation law d, q d) . ,q , q d, q (t) = (s, q , q It is desirable from a practical viewpoint, that the control law as well as the adaptation law, do not depend explicitly . on q
R. Kelly, V. Santiba nez and A. Loria 383
( t) = lim
Parametric convergence is not an intrinsic characteristic of an adaptive controller. Stability analysis is based on Lyapunov theory, with the inclusion of =
R. Kelly, V. Santiba nez and A. Loria
General form of the closed loop equation q d q , q d, q d, q d, = f t, q , q dt the origin is an equilibrium point. In general, is not the only equilibrium point We study only stability and convergence of the position errors: ( t) = 0 . limt q Certainty equivalence (the achievement of the control objective under parameter uncertainty)
R. Kelly, V. Santiba nez and A. Loria 385
(47)
The PD control with adaptive desired gravity compensation is described ) Kv q + g (q d , = Kpq + g (q ), Kv q + g (q d ) = Kpq 0 d and
t
(t) = g (q )T d
0
0 q q 1+ q
(0), ds +
where Kp, Kv IRnn and IRmm are symmetric positive denite design matrices 0 is a suitable positive constant. It was used (47) with x = q d.
R. Kelly, V. Santiba nez and A. Loria 387
Design parameters Only Kp and 0 must be chosen carefully. To that end, we start by dening Max{M }, kC 1 and kg as Max{M (q , )} Max{M } , ) kC 1 q C (q , q g (x, ) g (y , ) kg x y q IRn, IRm IRn, IRm q, q x, y IRn, IRm.
The constants Max{M }, kC 1 and kg are considered known. , ), g (q , ) and , It is necessary to dispose of M (q , ), C (q , q but one does not require to know . In practice the set is determined from upper and lowerbounds on the dynamic parameters
R. Kelly, V. Santiba nez and A. Loria 388
Kp and 0 are chosen so that C.1) C.2) C.3) C.4) min{Kp} > kg , 2min{Kp} > 0 , 2Max{M } 2min{Kv }[min{Kp} kg ] > 0 , 2 Max{Kv } min{Kv } > 0 2 [kC 1 + 2Max{M }]
= . We dene the parametric errors vector as: It is introduced only for analytical purposes (not used by the controller) It may be veried that = g (q ) + g (q ) g (q d ) d d + g (q , ) g (q ), = g (q d ) d 0 d The control law may be written as + g (q , ) . Kv q + g (q d ) = Kpq d Using above control law in the robot model, we obtain + g (q , ) g (q , ) . , )q = Kpq Kv q + g (q d ) M (q , ) q + C (q , q d
R. Kelly, V. Santiba nez and A. Loria 390
As = , we get that = (q )T g d 0 q . q 1+ q
From all the above we have that the closed loop equation is formed by q q C (q , q d M (q )1 Kpq )q + g (q d ) g (q ) ( q ) K q + v g q d = dt q g (q d)T 1+0 q q
Stability analysis
) = , q , V (t, q
q 0 q 0 1
393
The constants 0 > 0, 1 > 2 and 2 > 2 are chosen so that 2min{Kp} > 1 > 2 kg 21 2 = 1 2 2min{Kp} > 0 > 0 . 2Max{M } (48)
(49) (50)
Condition (48) guarantees that f ( q ) is a positive denite function (50) ensures that P is a positive denite matrix 1 1 1 Finally (49) implies that + = 2 2. 1
R. Kelly, V. Santiba nez and A. Loria 394
Dene as ) := = ( q
0 . 1+ q
2 M (q , ) = Kp 2M (q , ) > 0 2
395
+ + U (q , ) U (q d, ) + g (q d, )Tq
) f (q
which is obviously a positive denite function since 2 2 M (q , ) > 0 , K M (q , ) > 0, > 0, and p 2 f ( q ) is also a positive denite function (since min{Kp} > kg )
R. Kelly, V. Santiba nez and A. Loria 396
Time derivative of the Lyapunov function candidate ) = q (t, q q TKpq + q TM (q )q + q TKv q , q , TKv q V ) T [g (q d) g (q )] TC (q , q q q q . TM (q )q q It can be upper bounded, getting: T Q min{Kp} kg 1 2 Max {Kv } 1 2 Max {Kv }
1 20 min {Kv } 2
q q
) (t, q , q , V
q q
397
Q is positive denite if min{Kp} > kg 2min{Kv }(min{Kp} kg ) 2 Max {Kv } while we have that > 0 if min{Kv } > 0 . 2(kC1 + 2Max{M }) > 0
398
+ q
2
q 2
2 q q 0min{Q} 1+ q 2 It is a globally negative semidenite function. ) > 0 (globally) , q , Since moreover V (t, q
the origin of the closed loop equation is stable, and m , q Ln its solutions are bounded, that is: q and L .
R. Kelly, V. Santiba nez and A. Loria 399
Because:
2 d q ( t ) (t)) 0min{Q} ( t) , q ( t) , V (t, q , ( t) dt 1+ q
we have V0
0min{Q} 0 (0)). (0), q (0), where V0 := V (0, q q Ln That is: 2 . 1+ q Ln Using Lemma A.7 we obtain that: q 2
( t) 2 q dt , 1 + q ( t)
400
Example 15.2
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Dynamic model: ) C12(q , q ) M11(q ) M12(q ) C11(q , q g (q ) + + 1 = q q ) C22(q , q ) M21(q ) M22(q ) C21(q , q g 2 (q ) ) M (q ) g (q ) C (q ,q where
2 2 2 + m + l l M11(q ) = m1lc 2 1 1 c2 + 2l1 lc2 cos(q2 ) + I1 + I2 2 M12(q ) = m2 lc 2 + l1 lc2 cos(q2 ) + I2 2 M21(q ) = m2 lc 2 + l1 lc2 cos(q2 ) + I2 2 M22(q ) = m2lc 2 + I2
) = m2l1lc2 sin(q2) [q C12(q , q 1 + q 2] ) = m2l1lc2 sin(q2)q 1 C21(q , q ) = 0 C22(q , q g1(q ) = [m1lc1 + m2l1] g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) . We consider parametric uncertainty in m2, I2 and lc2; the numeric values of these constants are not known exactly. Nevertheless, we assume to know their upperbounds: m2, I2 and lc2, that is, m2 m2; I2 I2 ; lc2 lc2 .
Dynamic parameters vector IR3, m2 1 = 2 = m2lc2 . 2 3 m2lc 2 + I2 PD control with adaptive desired gravity compensation. + g (q ) Kv q + g (q d ) = Kpq 0 d
t 0
(t) = g (q )T d
0 q q 1+ q
(0) ds +
Kp, Kv IRnn and IRmm are symmetric positive denite design matrices 0 is a suitable positive constant.
R. Kelly, V. Santiba nez and A. Loria 404
Vector g 0(q d) g 0 (q d ) = Matrix g (q d) g (q d) = (q d, 0, 0, 0) = l1g sin(qd1) g sin(qd1 + qd2) 0 0 g sin(qd1 + qd2) 0 . m1lc1g sin(qd1) 0 .
It is necessary to characterize the set IR3, to which , as: = x1 2 x2 IR3 : x1 m2; x2 m2lc2; x3 m2lc2 + I2 . x3 Expressions for Max{M }, kC 1 and kg for the case of parametric uncertainty are: Max{M }
2 m1lc 1
2 m2 l1
+ 2lc2 + 3lc1lc2 + I1 + I2
kC 1 n2m2l1lc2 kg
R. Kelly, V. Santiba nez and A. Loria
Fixing the following values for the bounds m2 = 2.898 I2 = 0.0125 lc2 = 0.02862 [kg] kg m2 [m],
and considering the numeric values showed in table 5.1 we nally obtain: Max{M } = 0.475 kC 1 = 0.086 kg = 28.99 kg m2 kg m2 kg m2/sec2 .
407
The next step consists on calculating Kp, Kv and 0 and 2. Condition C.1: min{Kp} > kg As, kg = 28.99, hence Kp = diag{kp} = diag{30}. Kv is chosen arbitrarily but symmetric positive denite. We may x it to Kv = diag{kv } = diag{7, 3}. We chose 1 according with 2min{Kp} > 1 > 2, kg so an appropriate value is 1 = 2.01. 2 is determined from 21 , 2 = 1 2 so we get 2 = 402 .
R. Kelly, V. Santiba nez and A. Loria 408
Using above information, it is immediate to verify that 2min{Kp} 2Max{M } 2min{Kv }[min{Kp} kg ] 2 Max {Kv } min{Kv } 2 [kC 1 + 2Max{M }]
According to conditions C.2 through C.4, 0 must be strictly smaller than the previous quantities. Therefore, we choose 0 = 0.12. must be symmetric positive denite. A choice is e.g., = diag{1, 2} = diag{500, 10}.
R. Kelly, V. Santiba nez and A. Loria 409
The vector of initial adaptive parameters is arbitrary, and here it is taken (0) = 0. to be: In summary, the control law may be written as 1 + g sin(qd1 + qd2) 2 1 = kpq 1 kv q 1 + l1g sin(qd1) + m1lc1g sin(qd1) 2 . 2 = kpq 2 kv q 2 + g sin(qd1 + qd2) 3. Notice that the control law does not depend on
410
Consequently, the adaptation law only has the following two components:
t
0 q 1 q 1 1+ q
t
1(0) ds + ds 2(0) . ds +
0 q 1 q 1 1+ q
t
+ g sin(qd1 + qd2)
0
0 q 2 q 2 1+ q
In terms of the state vector of the closed loop equation, the initial state is 0.3141 /10 (0) q /30 0.1047 [rad] . = = 0 0 (0) q 0 0
412
[rad]
..... .... q ..... 1 ...... ...... .... 2 ................. .........q . ............ .................. ............................... ..................................... ...... .. .. . .. . .. . . . . . . . .. . . .. .. . . . .. .. .. .. . . ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ...... . . .. .. . . .. .. .. ..........................................................
0.1
0.0 12.5 25.0 37.5 50.0 t [sec]
Figure 100: Position errors q 1 and q 2. (t) tend Above Figure shows that the components of the position error q asymptotically to zero in spite of the nonmodeled friction phenomenon.
413
Control of Robot Manipulators in Joint Space 10.0 7.5 5.0 2.5 0.0 2.5 5.0 7.5 10.0
3.2902 1 ...................................................................... . . . . . . . . . . . . . . . . . . . . . . . . . . ........ .. . .. 0.1648 .. . ........................................................................................ .. . . .......................... .. . . ............................ ........... ... . . .. . ..... . .. . . . . . . . . . . . 2 ........ ... ..... . . . . .. .... .
0.0 12.5 25.0 37.5 50.0 t [sec]
1 and 2. Figure 101: Estimated parameters From Figure we appreciate that both parameters tend to values which are relatively near of the unknown values of 1 and 2, i.e., lim 1(t) 3.2902 1 m2 2.0458 = = = 2(t) 0.1648 0.047 2 m2lc2 .
In general, the parameters do not converge to their true values (persistency of excitation is not veried)
R. Kelly, V. Santiba nez and A. Loria 414
If instead of limiting the value of 0 we use the same gains as for the latter controllers, the performance is improved.
0.4 0.3 0.2 0.1 0.0
[rad] .... . . . q 1 . . . . . ... ...... .... ..... ... q . ...... 2 ........... ................. .......................................... . . ..... ..... ..... ..... ..... ..... . . .. ................. .. .. .........................................................................................................
0.1
0.0 0.5 1.0 1.5 2.0 t [sec]
415
Kv
and 0 = 5, i.e., Kp and Kv have the same values as for the PD controllers.
416
.. . . . . . . . . . . . . . . . . . . . . . . 1 . . .... ............................................... ..... ...................................................2.9053 ........................... . . . . . . . . . . . . . . . . .. . . . . . . . . 2 .................................................................... ....................................................0.1942 .......................... .. ...... . .. . . ... . . . . . . . . . . ..
0.0
0.5
1.0
1.5
2.0 t [sec]
417
The control and adaptation laws Firstly we recall that the PD controller with compensation is given by + C (q , q + M (q ) q ) [q d + + Kv q d + q = Kpq q ] + g (q ), where Kp, Kv IRnn are symmetric positive denite design matrices, = q d q denotes the position error, q and is dened as = Kv 1Kp .
R. Kelly, V. Santiba nez and A. Loria 418
Now we recall the following property Parameterization of the dynamic model M (q , )u + C (q , w , )v + g (q , ) = (q , u, v , w) + M0(q )u + C0(q , w)v + g 0(q ) where (q , u, v , w) IRnm, M0(q ) IRnn, C0(q , w) IRnn, g 0(q ) IRn and IRm. contains elements that depend on the dynamic parameters. M0(q ), C0(q , w) and g 0(q ) represent parts of ) and g (q ) that do not depend on . M (q ), C (q , q
419
IRm, we obtain For any vector ) = ) q ) [q + C (q , q , d + d + q q ] + g (q , M (q , + M0(q ) q ,q d + ) d + q d + q q, q (q , q ) [q d + q ] + g 0 (q ) . + C 0 (q , q where we dened d + q u = q d + q v = q . w = q ,q d + ). d + q q, q In the sequel we use the abbreviation: = (q , q
R. Kelly, V. Santiba nez and A. Loria 420
(51)
We are now ready to study the PD control with adaptive compensation Control law ) q ) [q + M (q , + C (q , q + Kv q , d + d + q = Kpq q] ) + g (q , (52)
(53)
+ T q q
(0), ds +
Kp, Kv IRnn and IRmm are symmetric positive denite. The pass from (52) to (53) follows by using (51).
R. Kelly, V. Santiba nez and A. Loria 421
= . We dene the parametric errors vector as: Introduced only with analytic purposes (it is not used by the controller). is unknown since it is a function of . It may be veried that = + + M (q , ) q + C (q , q , ) [q d + d + q = q ] + g (q , ) C 0 (q , q ) [q d + d + q q ] g 0 (q ) M0(q ) q
422
The control law takes the form + + Kv q = Kpq + C (q , q , ) [q d + d + q q ] + g (q , ) . + M (q , ) q Now the robot model can be expressed as: . + C (q , q + Kv q + q , ) q M (q , ) q q = Kpq Considering these facts we have that = T q + q .
R. Kelly, V. Santiba nez and A. Loria 423
which is a nonautonomous dierential equation and of which the origin, q = 0 IR2n+m , is an equilibrium point. q
R. Kelly, V. Santiba nez and A. Loria 424
Stability analysis
425
The time derivative of the Lyapunov function candidate becomes ) = (t, q , , q V (q , ) q + + + +1 q + q q q M (q , ) q q q M 2 T 1 T . + + 2 q Kpq
T T
TKv is symmetric positive denite (because, is a nonsingular matrix while Kv is a symmetric positive denite matrix, cf. Lemma 2.1) ) is a globally negative semidenite function. (t, q , , q Therefore, V
R. Kelly, V. Santiba nez and A. Loria 426
Above result, and since, the Lyapunov function candidate is globally positive denite, radially unbounded and decrescent, Theorem 2.3 guarantees that the origin of the closedloop equation is uniformly stable, and all the solutions are bounded, that is, Ln , q q , Lm .
427
Because d (t)) q ( t) , ( t) , q (t)TT Kv V (t, q q ( t) dt ( t) min{T Kv } q V0 T min{ Kv } (0)) (0), (0), q where V0 := (0, q Ln That is: q 2 . we have
( t) q
0
dt
428
Example 16.3
lc1
l1 x
I1 m1 q1 lc2 l2 m2 I2 q2
Link 2
Dynamic model:
) = m2l1lc2 sin(q2)q C11(q , q 2 ) = m2l1lc2 sin(q2) [q C12(q , q 1 + q 2] ) = m2l1lc2 sin(q2)q 1 C21(q , q ) = 0 C22(q , q g1(q ) = [m1lc1 + m2l1] g sin(q1) + m2lc2g sin(q1 + q2) g2(q ) = m2lc2g sin(q1 + q2) .
Unknown parameters: m2, I2 and lc2. = 0. We wish to design a controller, such that: limt q We use the PD Control with Adaptive Compensation.
R. Kelly, V. Santiba nez and A. Loria 431
21 = 0 22 = l1 cos(q2)u1 + l1 sin(q2)w1v1 + g sin(q1 + q2) 23 = u1 + u2 m2 2.0458 1 = 2 = m2lc2 = 0.047 2 0.0126 3 m2lc 2 + I2 M0(q ) = C 0 (q , w ) = g 0 (q ) =
2 m1lc 1 + I1 0
0 0
0 0 0 0 m1lc1g sin(q1) 0 .
433
Particularly: + C (q , q , ) [q d + d + q q ] + g (q , ) = M (q , ) q + g 0 (q ), d + q + M0(q ) q where 1 + 12q 2 u1 q d1 + 11q d + q = u = q = 1 + 22q 2 u2 q d2 + 21q d + q= v = q = w = q 1 + 12q 2 v1 q d1 + 11q = v2 q d2 + 21q 1 + 22q 2 .
q w1 = 1 w2 q 2
434
1(t) = 2 ( t) 3(t)
t 0
Where, Kp = diag{200, 150} [N m / rad] , Kv = diag{3} [N m sec / rad] , = diag{1.6 kg sec2/m2 , 0.004 kg sec2 , 0.004 kg m2 sec2 }, and therefore, = 11 21 12 1 := Kv Kp = diag{66.6, 50} [1/[sec]]. 22
Initial conditions, are chosen as: q1(0) = 0, q2(0) = 0 q 1(0) = 0, q 2(0) = 0 2(0) = 0 1(0) = 0, 3(0) = 0 .
436
0.01 0.02
[rad] ....... ..... q . ..1 . .. . . . . . . . . . ..... . ......... . . . . .. .. . ...... .... . . ..... . . . . .. ...... .. ........... ........... ............. . . . .. . .. ...... . ............ ... . . . .. ..... . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . .... ... . ...... ..... . .. .. . . ... .... ..... ... .... ...... . .... .. ...... . ... . . . .. ..... .... .. .. .. ... . .... .... ..... .... .. ...... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ......... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... ........ ..... ....... .... . ....... .... . .. q 2
10 t [sec]
Figure 105: Position errors. ( t) , Figure 105 shows the steady state tracking position errors q by virtue of friction phenomena in the actual robot, are not zero.
437
. . ........ .... ......... ....... ..... ......... . . . ..1 . . . . . . . . . . . . . . .. . .... .. .... .. .... .. .... .. ....... .... .. ....... ........ .. ....... ... . . ... .. . . . . .. . . .. . . .. .. . .. ....2 .............................................................................. .......................................................................... ....... 3 0 2 4 6 8 10 t [sec]
1, 2, and 3 Figure 106: Estimated parameters Figure 106 shows the evolution in time of the adaptive parameters. These parameters were arbitrarily assumed to be zero at the initial instant. We did not suppose having any knowledge a priori, about .
438
439
http://www.springer.com/9781852339944