100% found this document useful (1 vote)

505 views

Control of Robot Manipulators

Control of robot manipulators in joint space consists of three main steps: 1) Familiarization with the physical robot system, including its inputs, outputs, and environment interactions. 2) Deriving an accurate mathematical model of the robot system using physics laws or experimental data. 3) Defining control objectives such as trajectory tracking and specifying desired motions for the robot to perform. The book then introduces concepts from Lyapunov stability theory that are important to analyze robot control systems.

Uploaded by

Domagoj Majstorović

Available Formats

Download as PDF, TXT or read online on Scribd

100% found this document useful (1 vote)

505 views

Control of Robot Manipulators

Uploaded by

Domagoj Majstorović

Available Formats

Download as PDF, TXT or read online on Scribd

You are on page 1/ 441

CONTROL OF ROBOT MANIPULATORS IN JOINT SPACE

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

Control of Robot Manipulators in Joint Space

Part I PRELIMINARIES

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

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).

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Introduction

The International Federation of Robotics (norm ISO/TR 8373) denes:

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

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.

Figure 1: Robot manipulator.

R. Kelly, V. Santiba nez and A. Loria 6

Control of Robot Manipulators in Joint Space

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

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

1 2 . The torques and forces are collected in the vector: := . . n

R. Kelly, V. Santiba nez and A. Loria 8

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Familiarization with the physical system under consideration

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

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Figure 3: Robot interacting with its environment.

In this case, the output y may include the torques and forces f exerted by the end tool over its environment.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Image

Camera

Figure 5: Robotic system: camera in hand. Output y may correspond to the coordinates of the screen.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

q
-

ROBOT q

Figure 6: Input-output 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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Derivation of the dynamic model

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

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 of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Control specications

Denition of control objectives: Stability Regulation Trajectory tracking (motion control) Optimization.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

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. input-output 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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Motion control of robot manipulators

Point-to-point. Determines a series of points in the manipulators workspace, by which the end eector is required to pass.

Figure 7: Point-to-point motion specication.

R. Kelly, V. Santiba nez and A. Loria 19

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

(Continuous) trajectory. The control problem consists in making the end-eector follow a trajectory as closely as possible.

Figure 8: Trajectory motion specication. Set-point control problem. The specied trajectory is simply a point in the workspace.
R. Kelly, V. Santiba nez and A. Loria 20

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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 .

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

Preliminaries: Linear algebra

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

Euclidean norm The Euclidean norm x of a vector x IRn is dened as

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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

Spectral norm

The spectral norm A of a matrix A IRnm is dened as A = Max{ATA},

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

Theorem 2.1 Contraction Mapping. following continuous function

Consider IRm and the

f : IRn IRn x f (x , ) where stands for a vector of parameters. Assume that there exists a non-negative 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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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)

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

The concept of equilibrium

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

x1 1 x (t) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . .. . . . . . . . . . . . . . . .

x 62 x(0) = xe

x 62 x(0) = xe t
. . . .. . . .. . . .. . . .. . . . . . . . . . . . . .

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 .

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

Example 2.2
Consider a pendulum whose dynamic model is Jq + mgl sin(q ) = (t)
l g m

Figure 10: Pendulum.

R. Kelly, V. Santiba nez and A. Loria 33

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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)

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

Example 2.3
. . . . . ............... . . . . . . . . . . . . . . . ....... . . . . . . . . . ... .. .... . .. ........ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... .... . . . . . . . . . x . . . . e . . . . . . . . ... .................. . ..... . ... . . ........ . . . .. ............ ............... . . . .

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

x(0) x1 . .. .. . . . . .. . 1 . . . . . . .. . . . .. . . . . . . . .. . . .. . . . . . . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . .. . . . . . . .. . . .. . . .. . . .. . . .. . . .. . . . .. . .. . . . . . . . . . . .. . . .. . . .. . . . . . . . . .. .. . . . . . . . . . . . . . . .. . . . . . .. .. . .. . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x( t ) .. ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x . . . . . . . . . . . . . . . . . . . . . . . . . . . . e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . t . . . . . . . . . . . . . . . . . . .. . . . . ... . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . .

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

. . . . ...................... . . . . . . .............. .... . . . . . . . ....... ... . . ............... . . . . . . . . . . . . . . . .... . . . .. ... .. . ........... . . ... .... . .. . . ... ........ . . . . . . . .... ....... .. . .......... ... .... .. .... ............. . . . . . ................... . .. . ........ . .. . ....... ... . . . . . ..... ... . . . . . . ..................... ..... ..... ..... ..... ..... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . .. ..... .... .... .... .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . .... .......... . . . . . . . ..... ... .. . . .. . . . .. .. .. ......... .... .... ..... . . ...... ... .. .. . . . . . ...... ........ .... . . . . . . . . . ............................ . ............................................. . .. . . . . ................... . . . . . . . . . . . . . . . . . . . . . .

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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 .

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

. . ......... . . . . . . .. .. . . .. .. . . . . . . . ........... .. .. .. .. . .. .. . . . .. . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . .. . . . . . . . . . . . . . . . . .. . . . . . .. . .. . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ...... ..... ..... .. ..... ..... ..... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ........ . . . ..... .... . . . . . . . . . .. . . . . . . . .. . ....... .. . . . . ......... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. ... . ... . .. . . .... . . . ......... ................ ... . ........................ . . . . . .. . ..... . . ....... .............. ............. . . .............. . . .

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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 ).

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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 ).

If V (t, x) = V (x) then V (x) is decrescent.

R. Kelly, V. Santiba nez and A. Loria 48

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

. . . . . . . . . . . . V ( x ) . . . 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..... ...... . . . . . . . . . . . . . .. . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . . . . ..... ..... .... .. . ... . .... .... . ... .. ..... ..... . .... . . ... ..... ..... ...0 . . . . . . . . . . . . . . . . . . . . . . . ... .... . . .

. . ( x ) V . . . 2 . . . . . ... . . . . .. . . . . . . . . . . . . . . .. . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . .. .. . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . ... . ... . . . . . ... . .... .... .... .... ..... ..... ..... ... ... ..... ..... ..... ..... ..... ..... .... . . . .

x 0
. . . . . . . ........................... . . ........................... .. . . . . . . .. . . . . . . . . . . ... . . . . .. . .... ..... ..... .... .... .... .... .... ..... .. ..... ..... ..... ..... ..... ..... ...0 . . . . .

x 0 V4 (x) . . . . . . . . . . .. .. . . . . . . .. . . .. . ... . ... . . .. ... . . . . ... . .... . ... . . ..... . . . . ........ . .... .... .... .... ..... ....... .. .. ....... ........ ..... ..... ..... ..... ..... .... . . . . . . x 0

V3 (x)

x 0

Figure 16: Examples

R. Kelly, V. Santiba nez and A. Loria 50

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

is continuous with respect to t and x, is continuous with respect to t and x .

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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 .

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

Lyapunovs direct method

Theorem 2.2 Stability and uniform stability. = f (t, x), if there exists The origin is a stable equilibrium of Equation x V (t, 0) = 0 t 0 V (t, x) W1(x) > 0, (t, x) 0, V t0 t0 locally

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

W1(x) and W2(x) are positive denite functions.

R. Kelly, V. Santiba nez and A. Loria 55

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

Example 2.11
Consider the dynamic model of an ideal pendulum without friction Jq + mgl sin(q ) = 0 or, in the state-space 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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

It has multiple equilibria, at [q q ]T = [n 0]T for n = , 2, 1, 0, 1, 2, the origin is an equilibrium

x2
.......... ............. ............. .... .... .... .... . . . . . . . . . . . . . . . . . . . . . . . . .......... .. . . . . . . . . . . . . . . . . . . . ...... .......... ...... ........... ...... ............ . . . . . ... ... ... .. .. ...... .. .. .. ... ........... .. . . . .... . ... ... .. ............ .. ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... .... . ... .. ... . . ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .... . . . . . ... . . ... . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . .. . . . .... .. .... .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . . . . ... .. .. .. ... ... .. .. .... . ... ... ... ... ... . . . . . . . .. .. . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... ... .......... ...... ... ... ... ... . . . . . . . . ... .... ... ... . . . ... .... .. .. ... ..... ....... .... .............. .... .... ............. .... . . . . . . . . . . . . . . ....... ...... ....... ...... ....... ...... .... ... ...

Figure 17: Pendulum: Phase plane

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

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

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory

1. x(t), z (t) and h(t) are bounded for all t 0 and 2. x(t) is square-integrable, i.e.,

x ( t)
0

dt < .

is also bounded then we have that If moreover x lim x(t) = 0 .

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

We consider robot manipulators formed by an open kinematic chain.

z0 joint 2 q2 z1 link 1 joint 1 q1 q3 z2 link 2 z3 qn
x1 x.2 x= . . xm

zn joint n link n

y0 x0

Figure 18: Abstract diagram of an n-DOF robot manipulator.

R. Kelly, V. Santiba nez and A. Loria 66

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

Figure 19: Example of a 3-DOF robot.

R. Kelly, V. Santiba nez and A. Loria 67

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

The vector of joint positions q has n elements q1 q2 n q= IR . . . qn The position and orientation of the robots end-eector, 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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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)

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Lagranges equations of motion

Now we describe how to derive the dynamic model of a robot. Consider the robot manipulator with n links depicted in Figure 20.
z0 joint 2 q2 z1 link 1 joint 1 q1 q3 z2 link 2 z3 qn
x1 x.2 x= . . xm

zn joint n link n

y0 x0

Figure 20: Abstract diagram of an n-DOF robot manipulator.

R. Kelly, V. Santiba nez and A. Loria 71

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

) 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)

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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)

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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).

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Example 3.3

Consider the robot manipulator with 2 DOF shown in Figure 21.

z x l1 q1 I1 m1 1 lc1

q2 lc2

m2 I2

Figure 21: 2-DOF robot.

R. Kelly, V. Santiba nez and A. Loria 75

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

) 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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

) = K (q , q ) U (q ) , takes the form The Lagrangian L(q , q

) + K2(q , q ) ) = K1(q , q L(q , q 1 1 2 2 2 2 2 = (m1lc1 + m2l1 )q 1 + m2lc 2 2q 2 2 + m2l1lc2 cos(q1 q2 + )q 1q 2 1 2 1 2 + I1 q + I2 q . 2 1 2 2

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

By using Lagranges equations we have

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

and, q1 2 = m2l1lc2 cos(q1 q2 + )

2 + [m2lc q2 2 + I2 ] 2 m2l1lc2 sin(q1 q2 + )q 1 .
R. Kelly, V. Santiba nez and A. Loria 79

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Denoting C21 = cos(q2 q1), S21 = sin(q2 q1), one obtains 1 =

2 2 m1lc + m l 1 2 1 1 + I1 q

+ [m2l1lc2 cos( )C21 + m2l1lc2 sin( )S21] q2

2 + [m2l1lc2 cos( )S21 + m2l1lc2 sin( )C21]q 2

(8) and q1 2 = [m2l1lc2 cos( )C21 + m2l1lc2 sin( )S21]

2 + [m2lc q2 2 + I2] 2 + [m2l1lc2 cos( )S21 m2l1lc2 sin( )C21]q 1

(9)
R. Kelly, V. Santiba nez and A. Loria 80

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

which are of the form (4) with ,q , ) = f EL(q , q 1 RHS(8) 2 RHS(9)

where RHS(8) and RHS(9) denote the terms on the right hand side of (8) and (9) respectively.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Example 3.4
Consider the 3-DOF Cartesian robot manipulator shown in Figure 22.
z0 q2 z0 m3 q3 q1 y0 x0 x0 q2 m1 m2

q1 y0 q3

Figure 22: 3-DOF robot. ) = K (q , q

1 2 2 2 2 3 + [m1 + m2]q 2 + [m1 + m2 + m3]q 1 m1q .

U (q ) = [m1 + m2 + m3]gq1 .
R. Kelly, V. Santiba nez and A. Loria 82

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

In terms of the state vector [q1 q2 q3 q 1 q 2 q 3] , above equations may be expressed as q 2 q2 q 3 q3 d = . 1 dt [ [ m + m + m ] g ] 1 1 2 3 q 1 m1 + m2 + m3 1 q 2 2 m + m 1 2 1 q 3 3 m1

R. Kelly, V. Santiba nez and A. Loria 84

q 1

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Dynamic model in compact form

Consider a robot manipulator of n DOF composed of rigid links interconnected by frictionless joints.
T ) = 1 K (q , q M (q )q q 2

expresses the kinetic energy

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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

) 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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

)q + g (q ) = may be viewed as The model M (q ) q + C (q , q q

ROBOT q

Figure 23: Input-output representation of a robot.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Example 3.6

z x l1 q1 I1 m1 1 lc1

q2 lc2

m2 I2

Figure 24: 2-DOF robot.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

) = 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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

)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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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.

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Dynamic model of robots with actuators

Figure 25: 2-DOF 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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

They have their own dynamics,

Figure 26: 2-DOF robot.

R. Kelly, V. Santiba nez and A. Loria 97

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Actuators with linear dynamics

Ka , Kb , Ra Jm v fm q
1:r

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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

Jm : rotors inertia [kg m2], Ka : motor-torque 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),

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

R. Kelly, V. Santiba nez and A. Loria

100

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

Figure 28: Block-diagram of a robot with its actuators.

R. Kelly, V. Santiba nez and A. Loria

101

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

The equation above may also be written in the compact form + fLq + kL sin(q ) = JLq

where JL = J + ml2 kL = [mblb + ml]g .

R. Kelly, V. Santiba nez and A. Loria

104

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

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

where, we identify M (q ) = JL 1 R = r2 KaKb B = fm + Ra C (q, q ) = 0 = Jm Ka K = rRa f (q ) = fLq g (q ) = kL sin(q ) . J

R. Kelly, V. Santiba nez and A. Loria

105

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

When the gear ratios ri are suciently large, the robot-with-actuators 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 .

R. Kelly, V. Santiba nez and A. Loria

106

Control of Robot Manipulators in Joint Space

Ch. 3. Robot dynamics

] we have In terms of the state vector [q q

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 .

R. Kelly, V. Santiba nez and A. Loria

107

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

Dynamic model for n-DOF robots )q + g (q ) = . M (q ) q + C (q , q These properties, may be classied as follows. Properties of the inertia matrix M (q ), ), Properties of the centrifugal and Coriolis forces matrix C (q , q Properties of the gravitational forces and torques vector g (q ), Residual dynamics.
R. Kelly, V. Santiba nez and A. Loria 108

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

Properties of the inertia matrix

Property 4.1 Inertia matrix M (q ) The inertia matrix M (q ) = M (q )T > 0 IRnn, satises the following properties:

There exists a real positive number such that 1. M (q ) I q IRn

where I denotes the identity matrix of dimension n n. The matrix M (q )1 exists and is positive denite.

R. Kelly, V. Santiba nez and A. Loria

109

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

where Mij (q ) stands for the ij th element of the matrix M (q ).

R. Kelly, V. Santiba nez and A. Loria

110

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

R. Kelly, V. Santiba nez and A. Loria

111

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

R. Kelly, V. Santiba nez and A. Loria

112

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

The centrifugal and Coriolis forces matrix

) Property 4.2 Coriolis matrix C (q , q ) IRnn satises the The centrifugal and Coriolis forces matrix C (q , q following.

) may be not unique For a given manipulator, the matrix C (q , q )q is unique. but the vector C (q , q

C (q , 0) = 0 for all vectors q IRn.

R. Kelly, V. Santiba nez and A. Loria

113

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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 .

R. Kelly, V. Santiba nez and A. Loria

114

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

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 so-called Christoel symbol rst kind cjik (q ) and which is dened as cijk (q ) = Mkj (q ) Mij (q ) Mki (q ) . + qi qj q
k

R. Kelly, V. Santiba nez and A. Loria

115

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

R. Kelly, V. Santiba nez and A. Loria

116

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

R. Kelly, V. Santiba nez and A. Loria

117

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

), 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

R. Kelly, V. Santiba nez and A. Loria

118

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

) is derived, it always Independently of the way in which C (q , q satises q

1 ) q =0 M (q ) C (q , q 2

IRn . q, q

R. Kelly, V. Santiba nez and A. Loria

119

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

The vector of gravitational torques

Property 4.3 Gravity vector g (q ) The gravitational torques vector g (q ) IRn is continuous and therefore bounded for each bounded q and also satises the following.

are correlated as The vector g (q ) and the velocity vector q

1.
0

(t) dt = U (q (T )) U (q (0)) g (q (t))Tq for all T IR+ .

R. Kelly, V. Santiba nez and A. Loria

120

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

R. Kelly, V. Santiba nez and A. Loria

121

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

R. Kelly, V. Santiba nez and A. Loria

122

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

Furthermore, kg satises g (q ) g (q ) Max kg q q .

R. Kelly, V. Santiba nez and A. Loria

123

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

For robots having only revolute joints there exists a constant k such that g (q ) k for all q IRn.

R. Kelly, V. Santiba nez and A. Loria

124

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

R. Kelly, V. Santiba nez and A. Loria

125

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

Denition 4.1 Vectorial tangent hyperbolic function tanh(x1) . . tanh(x) := tanh(xn)

tanh(x)
1.0 0.5 0.0
. . ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... .. . . . . ...................................................... . . . . ........... . . . . . . . ... . . ...... . ... . .. . .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... .... . . . .... ..... ..... ..... ..... ..... ..... ..... .... .... .... .. ... . . . . ... . . . . . .. . . . . . . . ....... . . . . . . . . . . . . . . . . . . . . . . ..... ..... ..... ..... ..... ..... ..... ..... .... .... .... .. . . . ....................................... .. ..... ..... ..... ..... ..... . .

(12)

0.5 1.0

4 3 2 1

3 x 4

Figure 30: Tangent hyperbolic tanh(x).

R. Kelly, V. Santiba nez and A. Loria 126

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

R. Kelly, V. Santiba nez and A. Loria

127

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

where 1, , 4 > 0. With tanh(x) dened as in (12), the constants 1 = 1, 2 = n, 3 = 1, 4 = 1.

R. Kelly, V. Santiba nez and A. Loria 128

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

) 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

R. Kelly, V. Santiba nez and A. Loria

129

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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.

R. Kelly, V. Santiba nez and A. Loria

130

Control of Robot Manipulators in Joint Space

Ch. 4. Properties of the dynamic model

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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

The links are direct driven by two brushless DC motors.

y g
Link 1

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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

Table 1: Physical parameters of Pelican prototype manipulator.

Description Length of Link 1 Length of Link 2 Distance to the center of mass (Link 1) Distance to the center of mass (Link 2) Mass of Link 1 Mass of Link 2 Inertia rel. to center of mass (Link 1) Inertia rel. to center of mass (Link 2) Gravity acceleration Notation Value 0.26 0.26 0.0983 0.0229 6.5225 2.0458 0.1213 0.0116 9.81 Units m m m m kg kg kg m2 kg m2 m/sec2

l1 l2 lc1 lc2 m1 m2 I1 I2 g

R. Kelly, V. Santiba nez and A. Loria

134

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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) .

R. Kelly, V. Santiba nez and A. Loria

136

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

(q ) IR22 is called Jacobian matrix or, Jacobian of the J (q ) = q robot,

R. Kelly, V. Santiba nez and A. Loria

137

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

and the Relation of accelerations d x = J (q ) y dt where we have dened d J (q ) := dt (q ) q i. i =1 q i

q 1 q 1 + J (q ) q 2 q 2

R. Kelly, V. Santiba nez and A. Loria

138

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

Inverse kinematics
q1 = 1(x, y ), where 1 : IR2 and IR2. q2
y

It has the form

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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

qd1 x

Figure 34: No solution to the inverse kinematics problem.

y qd2

x qd1

Figure 35: No solution to the inverse kinematics problem.

R. Kelly, V. Santiba nez and A. Loria 140

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

The desired joint velocities may be obtained as q d1 q d2

R. Kelly, V. Santiba nez and A. Loria

= J 1(q d)

x d y d
141

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

and the desired joint accelerations as q d1 q d2 = J

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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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 end-eector of the robot is located at the physical boundary of the workspace
y qd2

x qd1

Figure 36: Bentover singular conguration.

R. Kelly, V. Santiba nez and A. Loria

143

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

Dynamic model

Lagrangian equations We start by writing the kinetic energy: ) = K1(q , q ) + K2(q , q ) K (q , q

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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

1 1 2 T m1v 1 v 1 + I1q 2 2 1 1 1 2 2 2 m1lc1q 1 + I1q . 2 2 1

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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) .

R. Kelly, V. Santiba nez and A. Loria

146

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

Applying the Lagranges equations that

R. Kelly, V. Santiba nez and A. Loria

i = 1, 2 we get

147

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

+ [m1lc1 + m2l1]g sin(q1) + m2glc2 sin(q1 + q2) and 2 =

2 2 m2lc + m l l cos( q ) + I + [ m l q2 q 2 1 c 2 2 2 1 2 2 c2 + I2] 2 + m2l1lc2 sin(q2)q 1 + m2glc2 sin(q1 + q2)

R. Kelly, V. Santiba nez and A. Loria

148

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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 ) = ,

R. Kelly, V. Santiba nez and A. Loria

149

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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

R. Kelly, V. Santiba nez and A. Loria

151

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

Table 2: Parameters for the Pelican prototype.

Max {M } n max Mij (q )
i,j,q

i,j,k,q

max

Mij (q) qk

k C1

max Ckij (q ) i,j,q

k C2

i,j,k,l,q

max

Ckij (q ) ql

max

gi (q ) i,j,q qj
152

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

Table 3: Numeric values of the parameters for the CICESE prototype.

Parameter Value 0.7193 0.0974 0.0487 0.0974 23.94 Units

Max {M } kM k C1 k C2 kg

kg m2 kg m2 kg m2 kg m2 kg m2 /sec2

R. Kelly, V. Santiba nez and A. Loria

153

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

Desired reference trajectories

We have selected the following reference trajectories in joint space: q d1 q d2 = b1[1 e

2.0 t3
3

] + 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

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

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]

Figure 37: Desired reference trajectories.

R. Kelly, V. Santiba nez and A. Loria

155

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

||qd (t)|| [rad] 2.0

1.5 1.0 0.5 0.0

10 t [sec]

Figure 38: Norm of the desired reference positions.

R. Kelly, V. Santiba nez and A. Loria

156

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

d(t)|| [ rad ||q sec ] . 2.4 .. . . .

1.8 1.2 0.6 0.0

.. . . . . . .... .... .... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. .. .. . . . . . . . . . . . . .. . . . ..

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

sin(1t) + [c1 c1e

2.0 t3

] cos (1t)1, (14)

157

2 2.0 t3

sin(2t) + [c2 c2e

2.0 t3

] cos (2t)2 .

in [ rad/sec ].
R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

d (t)|| [ rad ||q sec ] 10 .. . . .

8 6 4 2 0

.. . . . . . . . . . . . . . ... ... ... ... ... ... . . . .. . . . . .. .. . .. ... ... ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ... . ... . . . . . . .. . . . . . . ......... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . . . . . . . . . ... . ... ... . ... . ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

10 t [sec]

Figure 40: Norm of the desired reference accelerations vector.

R. Kelly, V. Santiba nez and A. Loria

158

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

Reference accelerations: q d1 = 12b1te

2.0 t3

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

sin (2t) + 12c2t e

2 ] sin (2t)2

2 2.0 t3

cos (2t)2

[c2 c2e

2.0 t3

rad / sec2 . (15)

R. Kelly, V. Santiba nez and A. Loria

159

Control of Robot Manipulators in Joint Space

Ch. 5. Case study: The Pelican prototype robot

From above gures we deduce the following upperbounds on the norms:

qd d q d q

Max Max Max

1.92 [rad] 2.33 [rad/sec] 9.52 [rad/sec] .

R. Kelly, V. Santiba nez and A. Loria

160

Control of Robot Manipulators in Joint Space

Part II SET-POINT CONTROL

R. Kelly, V. Santiba nez and A. Loria

161

Control of Robot Manipulators in Joint Space

Introduction

Introduction
Motion controllers are clasied Set-point 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

R. Kelly, V. Santiba nez and A. Loria

162

Control of Robot Manipulators in Joint Space

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.

R. Kelly, V. Santiba nez and A. Loria

163

Control of Robot Manipulators in Joint Space

Introduction

The objective of set-point control consists on nding such that

lim q (t) = q d

where q d IRn is a given constant vector which represents the desired set-point. 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

Control of Robot Manipulators in Joint Space

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

Figure 41: Set-point control: closed-loop system.

R. Kelly, V. Santiba nez and A. Loria 165

Control of Robot Manipulators in Joint Space

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 state-space form, i.e., d qd q q dt ), g (q )) . , q d , M (q ), C (q , q = f (q , q

CONTROLLER + ROBOT

q q

Figure 42: Set-point representation.

R. Kelly, V. Santiba nez and A. Loria

control

closed-loop

system:

Input-output

166

Control of Robot Manipulators in Joint Space

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.

R. Kelly, V. Santiba nez and A. Loria

167

Control of Robot Manipulators in Joint Space

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

R. Kelly, V. Santiba nez and A. Loria

168

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

qd Kp ROBOT q q

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

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

ROBOT

q q

Kv d q qd

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

R. Kelly, V. Santiba nez and A. Loria

170

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

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

= [sT 0T ]T where s IRn is solution of Kps g (q d s) = 0 .

R. Kelly, V. Santiba nez and A. Loria

171

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

Robots without gravity term

Dynamic model given by )q = . M (q ) q + C (q , q Assuming that q d is constant, the closed loop equation becomes q q d = dt )1 [Kpq , q )q ] Kv q C (q d q q M (q d q It represents an autonomous dierential equation (since q d is constant). T q T The origin q
R. Kelly, V. Santiba nez and A. Loria

= 0 is the only equilibrium of this equation.

172

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

To study the stability of the equilibrium, consider Lyapunov function candidate T q K 1 p 2 0 q 0 ) M (q d q q q

) = V ( q, q

1 T 1 T + q . M (q )q Kpq q 2 2

) is positive denite since M (q ) > 0 and Kp > 0. V ( q, q

R. Kelly, V. Santiba nez and A. Loria

173

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

) 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

R. Kelly, V. Santiba nez and A. Loria

174

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

the set is given by = = (x ) = 0 x IR2n : V x= q q ( ) = 0 q, q IR2n : V

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

R. Kelly, V. Santiba nez and A. Loria

175

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

Robots with gravity term

The closed loop equation is q q d . = dt Kv q C (q , q )q g (q )] q M (q )1 [Kpq

(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

= [sT 0T ]T where s IRn is solution of Kps g (q d s) = 0 .

R. Kelly, V. Santiba nez and A. Loria

176

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

Example 6.1
Consider the model of an ideal pendulum Jq + mgl sin(q ) = .

l g m

Figure 45: Pendulum.

R. Kelly, V. Santiba nez and A. Loria 177

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

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 .

R. Kelly, V. Santiba nez and A. Loria

178

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

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

For all vectors x, y , q d IRn, we get that f (x , q d ) f (y , q d ) kg xy min{Kp}

If min{Kp} > kg we have (by contraction mapping theorem) the only equilibrium is q
T

= s

R. Kelly, V. Santiba nez and A. Loria

179

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

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 non-negative 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

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

) yields The time derivative of V ( q, q 1 T ( +q ) = q TM (q ) +q TKpq T g (q ) , M (q )q V q, q q+ q 2

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

(t) 2dt < . q

181

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

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

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

Example 6.2
Consider again the ideal pendulum from Example 6.1 Jq + mgl sin(q ) = ,

l g m

Figure 46: Pendulum.

R. Kelly, V. Santiba nez and A. Loria 183

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

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

R. Kelly, V. Santiba nez and A. Loria

184

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

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 ]
..... ... ... ... ... ............ ...... ................................................................................................................ ... . . . . ....... ........ ..

20 t [sec]

Figure 47: Graph of q (t)2 .

R. Kelly, V. Santiba nez and A. Loria 185

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

and kp 2 2 q (0) = 0.61 q ( t) ml2

0.08 0.06 0.04 0.02 0.00
2 q (t)2 [( rad sec ) ]

rad sec

20 t [sec]

Figure 48: Graph of q ( t) 2

R. Kelly, V. Santiba nez and A. Loria

186

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

Is interesting to observe lim q (t) 1.25 = q ( t) 0 .

That is, the solutions tend to one of the three equilibria determined in Example 6.1

R. Kelly, V. Santiba nez and A. Loria

187

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

Example 6.3
Consider the 2-DOF 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

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

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

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

Consider the PD controller Kv q = Kpq with the following numeric values Kp = 30 0 , 0 30 Kv = 7 0 . 0 3

(0) = 0. The initial conditions are xed to q (0) = 0 and q

R. Kelly, V. Santiba nez and A. Loria

190

Control of Robot Manipulators in Joint Space

Ch. 6. Velocity feedback proportional control and PD control

0.4 0.3 0.2 0.1 0.0

[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

R. Kelly, V. Santiba nez and A. Loria

191

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

g (q ) q q Kv d q qd Kp

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

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

Global asymptotic stability by La Salles theorem

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

= 0 IR2n is the unique equilibrium.

R. Kelly, V. Santiba nez and A. Loria

193

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

To study the stability of the origin, consider Lyapunov function candidate T q K 1 p 2 0 q q 0 ) q M (q d q

) = V ( q, q

1 T 1 T + q M (q )q Kpq q 2 2

) is positive denite since M (q ) > 0 and Kp > 0 V ( q, q

R. Kelly, V. Santiba nez and A. Loria

194

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

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

R. Kelly, V. Santiba nez and A. Loria

195

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

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 set-point control Hence, limt q objective is achieved).
R. Kelly, V. Santiba nez and A. Loria 196

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

Example 7.1

Consider the Pelican robot studied in Chapter 5

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 52: Diagram of the Pelican robot.

R. Kelly, V. Santiba nez and A. Loria 197

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

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] .

The components of , are given by 1 kv q 1 + g1(q ) 1 = kpq 2 kv q 2 + g2(q ) . 2 = kpq

R. Kelly, V. Santiba nez and A. Loria

198

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

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

R. Kelly, V. Santiba nez and A. Loria

199

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

0.4 0.3 0.2 0.1 0.0

[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.

R. Kelly, V. Santiba nez and A. Loria

200

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

Lyapunov function for global asymptotic stability

It is convenient to cite some properties of the vectorial function. tanh(x) = [ tanh(x1) tanh(x2) tanh(xn)]T tanh(x)
1.0 0.5 0.0
. . ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... .... ..... .................................................. . ..... ..... ..... .. . ................ . . . . . . . ... . .... . . . . . .. . .. . ... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ... . . .... ..... ..... ..... ..... ..... ..... ..... .... .... .... .. . .... . . . .. . . ... . . . . .. . . . . . . . ... . . . . . . ..... ..... ..... ..... ..... ..... ..... ..... .... .... .... .. . . . . . . . . . . . . ............................................... .. ..... ..... ..... ..... ..... . .

0.5 1.0

4 3 2 1

Figure 54: Tangent hyperbolic function, tanh(x) where tanh(x) =

ex ex ex +ex

.
201

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

|x| | tanh(x)|, and 1 | tanh(x)| for all x IR therefore,

n x x IR tanh(x) n x IRn tanh(x) = 0 if and only if x = 0. For a symmetric positive denite matrix A, it has

1 T 1 min{A} tanh( Aq q) q 2 2 If moreover, A is diagonal, then

IRn q

min{A} tanh( q) tanh( q )T A q

R. Kelly, V. Santiba nez and A. Loria

IRn . q
202

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

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

1 Max {Kv } 2 . 1 { K } n k { M } v C1 Max min

204

Control of Robot Manipulators in Joint Space

Ch. 7. PD control with gravity compensation

( ) is a negative denite function because V q, q min{Kp} > 0 and

4min{Kp}min{Kv } > . 2 Max{Kv } + 4min{Kp}[ nkC 1 + Max{M }]

Finally, Theorem 2.4 allows to establish global asymptotic stability of the origin.

R. Kelly, V. Santiba nez and A. Loria

205

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

g (q d ) ROBOT q q

Kv d q qd

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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Considering the desired position q d to be constant, the closed-loop 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)

= 0 IR2n is an equilibrium point.

, 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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Example 8.1
Consider the model of an ideal pendulum Jq + mgl sin(q ) =

l g m

Figure 56: Pendulum.

R. Kelly, V. Santiba nez and A. Loria 208

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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

R. Kelly, V. Santiba nez and A. Loria

210

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Boundedness of position and velocity errors, q and q

Assume that Kp and Kv are positive denite (without assuming that Kp is suciently large). Then For a desired constant position q d, the closed loop equation has an equilibrium at the origin, but there might also be other equilibria. (t) and q (t) remain bounded for all initial conditions. In spite of this, q
R. Kelly, V. Santiba nez and A. Loria 211

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Dene the following non-negative function: 1 T ) = K (q , q ) + U (q ) kU + q Kpq V ( q, q 2 1 T 1 g (q d ) + q g (q d) + g (q d)TKp 2 where

T ) = 1 denotes the kinetic energy K (q , q M (q )q q 2

U (q ) denotes the potential energy and kU = minq {U (q )} .

R. Kelly, V. Santiba nez and A. Loria 212

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

) 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

(t) 2dt < . q

R. Kelly, V. Santiba nez and A. Loria

213

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

The explicit upper-bounds on the position and velocity errors are: g (q d ) + g (q d )

( t) q

(0)) + 2min{Kp}V ( q (0), q

min{Kp}

(23)

for all t 0, and ( t) q for all t 0.

(0)) 2V ( q (0), q min{M (q )}

(24)

R. Kelly, V. Santiba nez and A. Loria

214

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Example 8.2
Consider again the model of the ideal pendulum Jq + mgl sin(q ) = ,

l g m

Figure 57: Pendulum.

R. Kelly, V. Santiba nez and A. Loria 215

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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 ]
..... .. .. .. . . . . . . . . . ........... .. . ... ...... .......................................................................... . . ......... . . . . . .. . . .. . . ........ .............. ..............

30 t [sec]

Figure 58: Graph of position errors, q (t)2.

R. Kelly, V. Santiba nez and A. Loria 217

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

and

q 2 ( t)

2 kp 2 1 q (0) + mglq (0) + (mgl)2 7.75 J 2 2kp

rad sec

3 2 1 0

2 q (t)2 [( rad sec ) ]

30 t [sec]

Figure 59: Graph of velocities, q ( t) 2 .

R. Kelly, V. Santiba nez and A. Loria 218

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Evidence from simulation shows lim q (t) 4.57 = q ( t) 0 .

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 set-point control objective.

R. Kelly, V. Santiba nez and A. Loria

219

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Unicity of equilibrium
For robots having only revolute joints and considering the closed-loop equation (22), we have. The equilibria satisfy q
T

T T

= q

T T

IR2n where

1 = Kp ) g (q d)] = f ( IRn is solution of q [g (q d q q , q d) . q

For all vectors x, y IRn, we have that f (x , q d ) f (y , q d ) kg xy , min{Kp}

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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Global asymptotic stability

To study stability of the equilibrium of closed loop equation, consider min{Kp} > kg (the origin is the unique equilibrium) Lyapunov function candidate ) = V ( q, q 1 T )q + U (q d q ) U (q d ) M (q d q q 2 1 T T + q . Kpq + g (q d ) q 2

) 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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

( ) 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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Example 8.3

Consider the 2-DOF prototype robot studied in Chapter 5,

y g
Link 1

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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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) .

The constant kg may be obtained as kg = n max gi(q ) qj

i,j,q

= n [[m1lc1 + m2l1]g + m2lc2g ] = 23.94 kg m2/sec2 .

R. Kelly, V. Santiba nez and A. Loria

224

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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

R. Kelly, V. Santiba nez and A. Loria

225

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Figure shows the experimental results

0.4 0.3 0.2 0.1 0.0

[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.

R. Kelly, V. Santiba nez and A. Loria

227

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Lyapunov function for global asymptotic stability

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

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

which can be rewritten as ) = V ( q, q 1 T + U (q ) U (q d) + g (q d)Tq M (q )q q 2 1 1 0 T Kpq TM (q )q + q + q 1 2 1+ q

where 0 > 0, 1 > 2 and 2 > 2 are chosen so that 2min{Kp} > 1 > 2 kg 2 = 21 >2 1 2

2min{Kp} > 0 > 0 with ( Max{M (q )} ) . 2

R. Kelly, V. Santiba nez and A. Loria 229

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

To show positive denitness, we rearrange ) = V ( q, q 1 T 2 1 + q ]T M (q ) [q + q ] + q [q Kp 2M (q ) q 2 2 2 1 T T Kpq , + U (q ) U (q d ) + g (q d ) q + q 1 ) f (q .

with =

0 1+ q

) is positive denite because M (q ) > 0, and V ( q, q 2 2 K M (q ) > 0 and min{Kp} > kg . p 2

R. Kelly, V. Santiba nez and A. Loria

230

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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

min{Kp} kg 1 2 Max {Kv }

1 2 Max {Kv }
1 20 min {Kv } 2

q q

1 [min{Kv } 20(kC1 + 2 )] q 2

R. Kelly, V. Santiba nez and A. Loria

231

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

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 ,

R. Kelly, V. Santiba nez and A. Loria

232

Control of Robot Manipulators in Joint Space

Ch. 8. PD control with desired gravity compensation

Under this scenario, we get

( ) 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

R. Kelly, V. Santiba nez and A. Loria

233

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

ROBOT q q

Kv d q qd

t
0

Figure 62: PID control. + Ki + Kv q The PID control law is given by = Kpq
t ( ) q 0

d , where

Kp, Kv , Ki IRnn, are symmetric positive denite matrices.

R. Kelly, V. Santiba nez and A. Loria

234

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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

R. Kelly, V. Santiba nez and A. Loria

235

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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) .

R. Kelly, V. Santiba nez and A. Loria

236

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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

R. Kelly, V. Santiba nez and A. Loria

237

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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

R. Kelly, V. Santiba nez and A. Loria

238

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

The closed loop equation (25) may be expressed as d q = dt q q q q M (q )1

1 1 )q g (q ) + Kv q Kp Ki q Kiw + g (q d) C (q , q

. (26)
239

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

Above equation is autonomous T q T The origin of the state space, wT q equilibrium.

= 0 IR3n is the unique

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 set-point control objective is achieved locally.
R. Kelly, V. Santiba nez and A. Loria 240

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

Lyapunov function candidate

A positive denite Lyapunov function candidate is w 1 q 2 q T
1 Ki

, 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

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

Time derivative of the Lyapunov function candidate

It may be written as ( q T [Kp Ki] q , w ) = q T [Kv M (q )] q V q, q )T q T C (q , q q T [g (q d) g (q )] q It can be upperbounded by ( , w) V q, q where Q11 = [min{Kp} kg ] Max{Ki}, ) = min{Kv } [Max{M } + kC1 q ]. Q22(q q q
T

Q11 0

0 ) Q22(q

q q

R. Kelly, V. Santiba nez and A. Loria

242

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

It is possible to prove that there exists a ball D , q , w IRn : D := q w q q

( , 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.

R. Kelly, V. Santiba nez and A. Loria

243

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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 )}.

R. Kelly, V. Santiba nez and A. Loria

245

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

Example 9.2
Consider the 2-DOF prototype robot showed in Figure 63.
y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 63: Diagram of the Pelican prototype robot.

R. Kelly, V. Santiba nez and A. Loria

246

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

The elements of the inertia matrix M (q ) are

2 2 2 M11(q ) = m1lc 1 + m2 l1 + lc2 + 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 .

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).

R. Kelly, V. Santiba nez and A. Loria

247

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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

= n(m1lc1 + m2l1 + m2lc2)g = 23.94 kg m2/sec2

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

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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] .

R. Kelly, V. Santiba nez and A. Loria

249

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

0.4 0.3 0.2 0.1 0.0

[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

Control of Robot Manipulators in Joint Space

Ch. 9. PID control

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]

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

Control of Robot Manipulators in Joint Space

Part III TRACKING CONTROL

R. Kelly, V. Santiba nez and A. Loria

252

Control of Robot Manipulators in Joint Space

Introduction

Consider the dynamic model )q + g (q ) = . M (q ) q + C (q , q

T In terms of the state vector q T q

gives

q q d = dt )q g (q )] q M (q )1 [ (t) C (q , q

R. Kelly, V. Santiba nez and A. Loria

253

Control of Robot Manipulators in Joint Space

Introduction

R. Kelly, V. Santiba nez and A. Loria

254

Control of Robot Manipulators in Joint Space

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

Control of Robot Manipulators in Joint Space

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

Figure 66: Tracking control: closed loop system.

R. Kelly, V. Santiba nez and A. Loria

256

Control of Robot Manipulators in Joint Space

Introduction

d, q d , M (q ), C (q , q ), g (q )) . , q d, q = f (q , q

CONTROLLER + ROBOT

q q

Figure 67: Tracking representation.

R. Kelly, V. Santiba nez and A. Loria

control

closed-loop

system:

Input-output

257

Control of Robot Manipulators in Joint Space

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.

R. Kelly, V. Santiba nez and A. Loria

258

Control of Robot Manipulators in Joint Space

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.

R. Kelly, V. Santiba nez and A. Loria

259

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

In this chapter we study two controllers that do not present explicitly the linear PD term Computedtorque control, Computedtorque+ control.

R. Kelly, V. Santiba nez and A. Loria

260

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Computed-torque control
g (q ) d q M (q ) ROBOT q q

Kv d q qd

) C (q , q

Figure 68: Computed-torque control. The corresponding equation to computed-torque 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)

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

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

q I , Kv q

(28)

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

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.

R. Kelly, V. Santiba nez and A. Loria

263

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Consider next the globally positive denite Lyapunov function candidate ) = V ( q, q T q K + Kv 1 p 2 I q 1 + q q 2

I q I q

1 T + q + q Kp + Kv 2I q (29) q 2

where the constant satises, Kv I > 0 . Kp + Kv 2I > 0 .

R. Kelly, V. Santiba nez and A. Loria 264

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

) we get that Evaluating the total time derivative of V ( q, q

T q ) = q TKpq [Kv I ] q V ( q, q T q 0 Kp q . = q 0 Kv I q

(30)

( ) in (30) is globally negative denite. V q, q

R. Kelly, V. Santiba nez and A. Loria

265

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

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

For practical purposes, we can choose,

2 2 , , n Kp = diag 1

= diag {21, , 2n} .

R. Kelly, V. Santiba nez and A. Loria

266

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Example 10.2

Consider the Pelican prototype robot studied in Chapter 5

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 69: Diagram of the Pelican prototype.

R. Kelly, V. Santiba nez and A. Loria 267

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Consider the Computed-torque 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]

Figure 70: Desired reference trajectories.

R. Kelly, V. Santiba nez and A. Loria 268

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

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.

R. Kelly, V. Santiba nez and A. Loria

269

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

The symmetric positive denite matrices Kp and Kv are chosen as

2 2 , 2 } = diag{1500, 14000} [1 / sec] Kp = diag{1

= diag{21, 22} = diag{77.46, 236.64}

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 .

R. Kelly, V. Santiba nez and A. Loria

270

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

0.02 0.01 0.00

[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.

R. Kelly, V. Santiba nez and A. Loria

271

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Computed-torque+ control
g (q ) M (q ) ROBOT q q

d q

1 p+

) C (q , q

Kv Kp p p+ d q qd

Figure 72: Computed-torque+ control.

R. Kelly, V. Santiba nez and A. Loria 272

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

The equation corresponding to the computed-torque+ 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

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

The computed-torque+ 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)

R. Kelly, V. Santiba nez and A. Loria

274

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Now, we have the following closed loop equation q q

1 M (q ) C (q , q q Kv q Kpq ) 1 + 2 + q d , = dt + Kv q 1 + Kpq 1 2 2 q the origin q

T T T T q 1 2 T

(35)

= 0 IR4n is an equilibrium point.

R. Kelly, V. Santiba nez and A. Loria

275

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

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.

R. Kelly, V. Santiba nez and A. Loria

276

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Toward this end, we can get an equivalent closed loop equation + ] + C (q , q ) = 0 . M (q ) [ Consider now the following non-negative 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

R. Kelly, V. Santiba nez and A. Loria

277

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

We conclude
n L Ln 2 and

(t)T (t)

(t) 2 This means that that (t) 0 exponentially.

(0)) 2t 2V ( (0), q e .

Making use of the latter and of Corollary A.2 we get that

n Ln , q L q 2

( t) = 0 , lim q

R. Kelly, V. Santiba nez and A. Loria

278

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Example 10.3

Consider the 2-DOF prototype robot studied in Chapter 5

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 73: Diagram of the Pelican prototype.

R. Kelly, V. Santiba nez and A. Loria 279

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Consider the Computed-torque+ 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]

Figure 74: Desired reference trajectories.

R. Kelly, V. Santiba nez and A. Loria 280

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

Kp and Kv , and the constant are taken as

2 2 , 2 } = diag{1500, 14000} [1 / sec] Kp = diag{1

= diag{21, 22} = diag{77.46, 236.64}

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

Control of Robot Manipulators in Joint Space

Ch. 10. Computed-torque control and Computed-torque+ control

0.02 0.01 0.00

[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.

R. Kelly, V. Santiba nez and A. Loria

282

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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.

R. Kelly, V. Santiba nez and A. Loria

283

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

PD Control with Compensation

g (q ) d q M (q ) ROBOT q q

) Kv C (q , q

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)

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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

q q d = dt C (q , q + Kv q ) q M (q )1 Kpq q q It is non-autonomous and has the origin q

T T

= 0 IR2n as an equilibrium point.

R. Kelly, V. Santiba nez and A. Loria

285

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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)

R. Kelly, V. Santiba nez and A. Loria

286

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

which is equivalent to 1 V (t, q , q ) = 2 q q

I T 0 I

Kp 0 0 M (q ) B T AB

I 0 I

q q

It is easy to see ) 1 Min{B T AB } , q V (t, q 2 + q q

It is positive denite (because Kp > 0, Kv > 0 and hence B T AB > 0) and radially unbounded.

R. Kelly, V. Santiba nez and A. Loria

287

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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

R. Kelly, V. Santiba nez and A. Loria

288

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

The time derivative of the Lyapunov function candidate (37) is

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.

R. Kelly, V. Santiba nez and A. Loria

289

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

Example 11.1

Consider the Pelican robot presented in Chapter 5

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 77: Diagram of the Pelican robot.

R. Kelly, V. Santiba nez and A. Loria 290

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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]

Figure 78: Desired reference trajectories.

R. Kelly, V. Santiba nez and A. Loria

291

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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

R. Kelly, V. Santiba nez and A. Loria

292

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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] ,

1 Kp = diag{66.6, 50} [1/sec]. and therefore = Kv

The initial conditions corresponding a the positions and velocities are q1(0) = 0, q2(0) = 0 2(0) = 0 . q 1(0) = 0, q

R. Kelly, V. Santiba nez and A. Loria

293

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

0.02 0.01 0.00

[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.

R. Kelly, V. Santiba nez and A. Loria

294

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

PD+ control
g (q ) d q M (q ) ROBOT q q

) C (q , q d q qd

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)

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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.

R. Kelly, V. Santiba nez and A. Loria

296

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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

which is positive denite since M (q ) > 0 and Kp > 0.

R. Kelly, V. Santiba nez and A. Loria

297

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

Taking the time derivative of (39) we obtain

(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 square-integrable (
0

( t) q

dt < . )

R. Kelly, V. Santiba nez and A. Loria

298

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

Example 11.3

Consider the 2-DOF prototype robot studied in Chapter 5

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 81: Diagram of the Pelican robot.

R. Kelly, V. Santiba nez and A. Loria 299

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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]

Figure 82: Desired reference trajectories.

R. Kelly, V. Santiba nez and A. Loria

300

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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

R. Kelly, V. Santiba nez and A. Loria

301

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

0.02 0.01 0.00

[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.

R. Kelly, V. Santiba nez and A. Loria

302

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

Lyapunov function for asymptotic stability

We present an alternative stability analysis. Consider now, Lyapunov function candidate, T Kp q 1 0 2 q M (q ) 1+ q
0 1+ q

M (q )

) = , q V (t, q

M (q )

q (40) q

) + , q = W (t, q

0 T M (q )q q 1+ q ( q)

T + 1q TM (q )q ) = 1q , q is the Lyapunov function Kpq where W (t, q 2 2 (39).

R. Kelly, V. Santiba nez and A. Loria 303

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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

which is positive denite since so is M (q ) as well as Kp 2M (q ).

R. Kelly, V. Santiba nez and A. Loria 304

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

) satises , q Notice that V (t, q 1 , q ) V (t, q 2 q q

0 Max{M } Max{Kp} 0 Max{M } Max{M }

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

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

The time derivative of the Lyapunov function candidate (40) is given by

( ) = 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

R. Kelly, V. Santiba nez and A. Loria

306

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

It may be upperbounded in the following manner T Q

q q

(t, q ) , q V

) , q h( q 1 [min{Kv } 20 (kC1 + 2Max{M })] q 2

R. Kelly, V. Santiba nez and A. Loria

307

Control of Robot Manipulators in Joint Space

Ch. 11. PD+ Control and PD Control with Compensation

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 }

(t, q ) is negative denite , q The function V

T q Thus, using Theorem 2.4 we conclude that the origin q T T

IR2n is globally uniformly asymptotically stable.

R. Kelly, V. Santiba nez and A. Loria

308

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

Practical implementation of controllers is via digital technology. , Sampling of the joint position q and of the velocity q computation of the control action from the control law, the order to apply this control action is sent to the actuators. d(t) and q d ( t) , Control strategies using precomputed terms based on q d(t), q has advantages: Reduction in the time of computation of Higher processing frequency (larger potential for fast tasks)
R. Kelly, V. Santiba nez and A. Loria 309

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

Feedforward control
d q M (q d ) ROBOT q q

d) C (q d , q

g (q d )

d q qd

Figure 84: Feedforward control. The feedforward controller is given by d )q d + g (q d ) . q d + C (q d , q = M (q d) (42)

R. Kelly, V. Santiba nez and A. Loria

310

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

Example 12.3

Consider the 2-DOF prototype robot studied in Chapter 5

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 85: 2-DOF robot.

R. Kelly, V. Santiba nez and A. Loria 312

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

2.0 1.5 1.0 0.5 0.0

[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

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

PD plus feedforward control

d q M (q d ) ROBOT q q

d) C (q d , q

g (q d )

d q qd

Figure 88: PD plus feedforward control. Control law is given by + M (q d) d )q d + g (q d ), + Kv q q d + C (q d , q = Kpq

T T > 0, Kv = Kv > 0 IRnn. where Kp = Kp

R. Kelly, V. Santiba nez and A. Loria

315

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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 so-called 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)

However, the number of equilibria depends on the proportional gain Kp.

R. Kelly, V. Santiba nez and A. Loria 316

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

Unicity of the equilibrium

The equilibria are the constant vectors T ]T = [ [q T q q T 0T ]T IR2n, IRn is a solution of Kpq + h( q , 0) = 0 . where q = 0 IRn It always is satised by the trivial solution q Explicit conditions to ensure unicity of the equilibrium are presented next.
1 Dene k( q ) = Kp h( q , 0) . For all vectors x, y IRn, we have

k (x ) k (y )

1 Kp [h(x, 0) h(y , 0)]

1 Max Kp
R. Kelly, V. Santiba nez and A. Loria

h(x, 0) h(y , 0) .
317

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

On the other hand, we have that h(x, 0) h(y , 0) d [M (q d y ) M (q d x)] q d ) C (q d x , q d)] q d + [C (q d y , q + g (q d y ) g (q d x ) .

d h(x, 0) h(y , 0) kg + kM q

d M + kC2 q

2 M

xy .

R. Kelly, V. Santiba nez and A. Loria

318

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

We get that k (x ) k (y ) 1 d kg + kM q min {Kp} d M + kC2 q

2 M

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).

R. Kelly, V. Santiba nez and A. Loria

319

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

Global uniform asymptotic stability

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

R. Kelly, V. Santiba nez and A. Loria

320

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

so that

2 2 2 1 Max {M } Max{Kp} min{Kp} > min{M }

(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 .

R. Kelly, V. Santiba nez and A. Loria

321

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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)

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

The Lyapunov function candidate (45) satises the following inequality ) 1 , q V (t, q 2 q q
T

1 Max{M } min{Kp} 1 Max{M } min{M }

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 }

R. Kelly, V. Santiba nez and A. Loria

323

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

One may also show 1 , q ) V (t, q 2 q q

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

R. Kelly, V. Santiba nez and A. Loria

324

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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

R. Kelly, V. Santiba nez and A. Loria

325

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

which can be upperbounded by (t, q , q ) V min {Kp } T kh2 tanh( q) 3 1 kh2 q a 2 where a =

1 kh2 a 2 1 [min {Kv } kh1] b

tanh( q) q

R ( ) 1 d [Max{Kv } + kC1 q 2 b = 4 Max{M } + 2 kC1 .

+ 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

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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

from the specication of a given task

R. Kelly, V. Santiba nez and A. Loria

327

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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.

R. Kelly, V. Santiba nez and A. Loria

329

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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

R. Kelly, V. Santiba nez and A. Loria

330

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

Example 12.5

Consider the 2-DOF prototype robot showed in the Figure

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 89: 2-DOF robot.

R. Kelly, V. Santiba nez and A. Loria 331

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

The elements of the inertia matrix M (q ) are

2 2 2 M11(q ) = m1lc 1 + m2 l1 + lc2 + 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 .

R. Kelly, V. Santiba nez and A. Loria

332

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

) 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) .

R. Kelly, V. Santiba nez and A. Loria

333

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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 ,

min{M (q )} = 0.011 kg m2 . Numerically, we get: k = 7.664 d q [N m] , and d q

Max

Max = 2.33 [ rad/sec] and,

= 9.52 [ rad/sec2] .
334

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

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 .

Finally, we set = 2 [sec1],

R. Kelly, V. Santiba nez and A. Loria 335

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

An appropriate choice of the gains is Kp = diag{200, 150} [N m] , Kv = diag{3} [N m sec/rad] .

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

R. Kelly, V. Santiba nez and A. Loria

336

Control of Robot Manipulators in Joint Space

Ch. 12. Feedforward control and PD plus feedforward control

0.02 0.01 0.00

[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.

R. Kelly, V. Santiba nez and A. Loria

337

Control of Robot Manipulators in Joint Space

Part IV ADVANCED TOPICS

R. Kelly, V. Santiba nez and A. Loria

338

Control of Robot Manipulators in Joint Space

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

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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.

R. Kelly, V. Santiba nez and A. Loria

340

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

Specically, IRn is given by: bip = diag q p + ai

d , ai and bi are arbitrary strictly positive real constants, where, p = dt for i = 1, 2, , n.

A state-space 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

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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.

R. Kelly, V. Santiba nez and A. Loria

343

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

PD Control with gravity compensation

Control law: d ] + g (q ) + Kv [q = Kpq

= 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

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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

R. Kelly, V. Santiba nez and A. Loria

345

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

it veries the set-point 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

= 0 IR3n is the unique equilibrium.

R. Kelly, V. Santiba nez and A. Loria

346

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

Stability of the origin, Lyapunov function candidate 1 T 1 )T Kv B 1 ( B q ) , q ) = K (q , q ) + q + ( B q Kpq V ( , q 2 2 where

T ) = 1 is the kinetic energy function K (q , q M (q )q q 2 Kv B 1 is positive denite. , q ) is globally positive denite. Hence, V (, q

R. Kelly, V. Santiba nez and A. Loria

347

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

Example 13.1

Consider the Pelican robot

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 92: Diagram of the Pelican robot.

R. Kelly, V. Santiba nez and A. Loria 349

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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] ,

R. Kelly, V. Santiba nez and A. Loria

350

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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 .

Initial conditions q1(0) = 0, q2(0) = 0 2(0) = 0 q 1(0) = 0, q x1(0) = 0, x2(0) = 0 .

R. Kelly, V. Santiba nez and A. Loria 351

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

The desired joint positions are chosen as

qd1 = /10, qd2 = /30 [rad] .

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

Ch. 13. PD Control

[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 non-modeled friction eects).

R. Kelly, V. Santiba nez and A. Loria

353

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

PD Control with desired gravity compensation

Control law d ] + g (q d ) + Kv [q = Kpq

= 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

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

If min{Kp} > kg , then it veries the set-point 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

= 0 IR3n is the unique equilibrium

R. Kelly, V. Santiba nez and A. Loria

356

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

Stability of the origin Consider the Lyapunov function candidate 1 )T Kv B 1 ( B q , q ) + f ( ) , q ) = K (q d q q ) + ( B q V ( , q 2 where

T , q ) = 1 )q K (q d q M (q d q q 2 T ) U (q d ) + g (q d )T q +1 . f ( q ) = U (q d q Kpq q 2 Since Kv B 1 is positive denite and min{Kp} > kg , q ) is also globally positive denite. Consequently, V (, q

R. Kelly, V. Santiba nez and A. Loria

357

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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).

R. Kelly, V. Santiba nez and A. Loria

358

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

Example 13.2

Consider the Pelican robot

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 95: Diagram of the Pelican robot.

R. Kelly, V. Santiba nez and A. Loria 359

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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) .

The constant kg may be obtained as (see Property , 4.3): gi(q ) qj

= n

max

i,j,q

= n((m1lc1 + m2l1)g + m2lc2g ) = 23.94

R. Kelly, V. Santiba nez and A. Loria

kg m2/sec

.
360

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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] ,

R. Kelly, V. Santiba nez and A. Loria

361

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

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 .

Initial conditions q1(0) = 0, q2(0) = 0 2(0) = 0 q 1(0) = 0, q x1(0) = 0, x2(0) = 0 .

R. Kelly, V. Santiba nez and A. Loria 362

Control of Robot Manipulators in Joint Space

Ch. 13. PD Control

The desired joint positions are

qd1 = /10, qd2 = /30 [rad] .

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

Ch. 13. PD Control

[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).

R. Kelly, V. Santiba nez and A. Loria

364

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

Ch.14. Introduction to adaptive robot control

The implicit assumptions in the preceding chapters are: The model is accurately known The constant physical parameters are accurately known. Two general techniques deal with the absence of above considerations: Robust control aims at controlling, with a small error, a class of robot manipulators (model is not accurately known) with the same controller. Adaptive control deals with uncertainty in the systems parameters (unknown constant parameters) it requires the precise knowledge of the structure of the system.
R. Kelly, V. Santiba nez and A. Loria 365

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

In the subsequent chapters we describe and analyze two adaptive controllers for robots.

PD control with feedforward gravity compensation. PD control with adaptive compensation.

R. Kelly, V. Santiba nez and A. Loria

366

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

Parameterization of the dynamic model

The dynamic model of robot manipulators as we know, is given by )q + g (q ) = M (q ) q + C (q , q To emphasize the dependence of the dynamic model on the dynamic parameters, we write , )q + g (q , ) = . M (q , ) q + C (q , q where IRm is the m vector of parameters m is some known constant , do not necessarily correspond to the physical parameters of the robot
R. Kelly, V. Santiba nez and A. Loria 367

(46)

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

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

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

Linearity in the Dynamic Parameters

Above example also shows that the dynamic model is linear in the parameters . ml q + mgl sin(q )
2

[ 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

the

parameters

linear

369

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

Property 14.1. Linearity in the dynamic parameters.

For all u, v , w IRn it holds that M (q , )u+C (q , w, )v +g (q , ) = (q , u, v , w)+(q , u, v , w) 1. where (q , u, v , w) is a vector of n 1, (q , u, v , w) is a matrix of n m and the vector IRm depends only on the dynamic parameters of the manipulator and its load.

nm Moreover, if q , u, v , w Ln . then (q , u, v , w ) L

R. Kelly, V. Santiba nez and A. Loria

370

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

The Nominal Model

The dynamics (46), may be expressed as M (q , )u + C (q , w , )v + g (q , ) = (q , u, v , w) + M0(q )u + C0(q , w)v + g 0(q ),

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

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

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 ) .

R. Kelly, V. Santiba nez and A. Loria

372

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

Example 14.7

Consider the Pelican robot

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 97: Diagram of the Pelican robot.

R. Kelly, V. Santiba nez and A. Loria 373

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

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 C11(q , q 2

R. Kelly, V. Santiba nez and A. Loria 374

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

) = 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

v1 v2

w1 w2

.
375

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

The parameterization leads to M (q , )u + C (q , w , )v + g (q , ) = 11 12 13 1 2 + M0(q )u + C0(q , w)v + g 0(q ), 21 22 23 3 where

2 11 = l1 u1 + l1g sin(q1)

12 = 2l1 cos(q2)u1 + l1 cos(q2)u2 l1 sin(q2)w2v1 l1 sin(q2)[w1 + w2]v2 + g sin(q1 + q2) 13 = u1 + u2 21 = 0

R. Kelly, V. Santiba nez and A. Loria 376

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

22 = l1 cos(q2)u1 + l1 sin(q2)w1v1 + g sin(q1 + q2) 23 = u1 + u2 m2 1 = 2 = m2lc2 2 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 .

depends exclusively on the parameters of interest m2, I2 and lc2.

R. Kelly, V. Santiba nez and A. Loria 377

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

The Adaptive Robot Control Problem

Consider the dynamic equation , )q + g (q , ) = M (q , ) q + C (q , q or equivalently, )q + g 0 (q ) = . , q ,q ) + M0(q ) q + C 0 (q , q (q , q ) IRnn and , q ,q ) IRnm, M0(q ), C0(q , q Assume that (q , q g 0(q ) IRn are known but IRm is unknown d and q d, Given a set of vectorial bounded functions q d, q we seek to design controllers that achieve the position or the motion control objectives.
R. Kelly, V. Santiba nez and A. Loria 378

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

Parameterization of the controller

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

R. Kelly, V. Santiba nez and A. Loria

379

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

, 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

R. Kelly, V. Santiba nez and A. Loria

380

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

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

where is the vector of adaptive parameters

R. Kelly, V. Santiba nez and A. Loria 381

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

An adaptation law (integral law or gradient type)

( 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 .

R. Kelly, V. Santiba nez and A. Loria

382

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

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: block-diagram. 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

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

Stability and Convergence of Adaptive Control Systems

An adaptive system guarantees the parametric convergence if (t) when t exists and is such that the limit of
t

( 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

(parametric errors vector)

384

Control of Robot Manipulators in Joint Space

Ch. 14. Introduction to adaptive robot control

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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

Ch.15. PD Control with Adaptive Desired Gravity Compensation

We consider the scenario where all the joints of the robot are revolute. The Control and Adaptive Laws IRm and x IRn Making use of (47), we have that for any vector ) = (x, 0, 0, 0) + g (x ) . g (x , 0 For simplicity, in the sequel we use g (x) = (x, 0, 0, 0) .
R. Kelly, V. Santiba nez and A. Loria 386

(47)

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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 }]

where 2 and 1 are dened so that 21 2 = 1 2 2min{Kp} > 1 > 2 . kg

R. Kelly, V. Santiba nez and A. Loria 389

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

= . 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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

Autonomous dierential equation (the origin is an equilibrium point).

R. Kelly, V. Santiba nez and A. Loria 391

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

Stability analysis

Lyapunov function candidate P T q 0 M (q , ) 2 K p 2 1+ q 1 q 0 M (q , ) 2 M (q , ) 1+ q 0 0 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 392

) = , q , V (t, q

q 0 q 0 1

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

Equivalent form ) = , q , V (t, q 1 T + U (q , ) U (q d, ) + g (q d, )Tq M (q , )q q 2 1 1 0 T TM (q , )q Kpq + + q q 1 2 1+ q 1 T 1 + , 2

R. Kelly, V. Santiba nez and A. Loria

393

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

Dene as ) := = ( q

0 . 1+ q

Inequality (50) implies that the matrix 2 Kp 2 0 1+ q

2 M (q , ) = Kp 2M (q , ) > 0 2

R. Kelly, V. Santiba nez and A. Loria

395

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

The Lyapunov function candidate may be rewritten as, ) = , q , V (t, q 1 + q ]T M (q , ) [q + q ] [q 2 1 T 2 + q Kp 2M (q , ) q 2 2 1 T 1 + 2 1 T Kpq , q 1

+ + 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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

1 [min{Kv } 20(kC1 + 2Max{M })] q 2

R. Kelly, V. Santiba nez and A. Loria

397

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

R. Kelly, V. Santiba nez and A. Loria

398

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

Finally we obtain that ) (t, q , q , V 0 min{Q} 1+ q q

+ 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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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)

n , q Ln Thus, from q and q L 2 , and Lemma A.5 we obtain

(t) = 0 IRn (control objective achieved). lim q

R. Kelly, V. Santiba nez and A. Loria

400

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

Example 15.2

Consider the Pelican robot

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 99: Diagram of the Pelican robot.

R. Kelly, V. Santiba nez and A. Loria 401

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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 C11(q , q 2

R. Kelly, V. Santiba nez and A. Loria 402

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

) = 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 upper-bounds: m2, I2 and lc2, that is, m2 m2; I2 I2 ; lc2 lc2 .

(t) = 0 (with a constant q d). The control problem: limt q

R. Kelly, V. Santiba nez and A. Loria 403

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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 .

We rst need to determine the numeric values of Max{M (q , )} Max{M } , ) kC 1 q C (q , q g (x, ) g (y , ) kg x y

R. Kelly, V. Santiba nez and A. Loria

q IRn, IRn, q, q x, y IRn, .

405

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

n m1lc1 + m2l1 + m2lc2 g .

406

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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 .

R. Kelly, V. Santiba nez and A. Loria

407

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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 }]

= 0.561 = 0.124 = 1.448

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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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

R. Kelly, V. Santiba nez and A. Loria

410

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

Consequently, the adaptation law only has the following two components:
t

1(t) = l1g sin(qd1)

0 q 1 q 1 1+ q
t

1(0) ds + ds 2(0) . ds +

2(t) = g sin(qd1 + qd2)

0 q 1 q 1 1+ q
t

+ g sin(qd1 + qd2)
0

0 q 2 q 2 1+ q

Laboratory experimental results. Initial conditions: q1(0) = 0, q2(0) = 0 q 1(0) = 0, q 2(0) = 0 .

R. Kelly, V. Santiba nez and A. Loria 411

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

Desired joint positions: qd1 = /10, qd2 = /30 [rad] .

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

R. Kelly, V. Santiba nez and A. Loria

412

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

0.4 0.3 0.2 0.1 0.0

[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 non-modeled friction phenomenon.

R. Kelly, V. Santiba nez and A. Loria

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

Ch. 15. PD control with adaptive desired gravity compensation

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

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

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]

Figure 102: Position errors q 1 and q 2.

R. Kelly, V. Santiba nez and A. Loria

415

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

For this, we set the gains to Kp = 30 0 7 0 500 0 0 3 10 10 0 30 [Nm / rad] ,

[Nm sec / rad] ,

[Nm / (rad sec)] ,

and 0 = 5, i.e., Kp and Kv have the same values as for the PD controllers.

R. Kelly, V. Santiba nez and A. Loria

416

Control of Robot Manipulators in Joint Space

Ch. 15. PD control with adaptive desired gravity compensation

10.0 7.5 5.0 2.5 0.0 2.5 5.0 7.5 10.0

.. . . . . . . . . . . . . . . . . . . . . . . 1 . . .... ............................................... ..... ...................................................2.9053 ........................... . . . . . . . . . . . . . . . . .. . . . . . . . . 2 .................................................................... ....................................................0.1942 .......................... .. ...... . .. . . ... . . . . . . . . . . ..

0.0

0.5

1.0

1.5

2.0 t [sec]

1 and 2. Figure 103: Estimated parameters

R. Kelly, V. Santiba nez and A. Loria

417

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

Ch.16. PD Control with Adaptive Compensation

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

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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

R. Kelly, V. Santiba nez and A. Loria

419

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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)

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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)

+ M0(q ) q + C 0 (q , q + ) [q d + + Kv q d + q q] = Kpq + g 0 (q ), and ( t) =

(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

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

= . 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

R. Kelly, V. Santiba nez and A. Loria

422

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

Finally, the closed-loop equation, may be written as: q q d C (q , q + M (q , )1 Kpq Kv q , ) q q = q dt + q T q

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

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

Stability analysis

Lyapunov function candidate T 2Kp + TM (q , ) TM (q , ) q 1 , q , ) = q V (t, q M (q , ) M (q , ) 2 0 0 q 0 . 0 q 1

which is positive denite. This, may be more clear when rewriting it as

T 1 1 T 1 T , q , ) = q + . + + Kpq V (t, q q M (q , ) q q +q 2 2

R. Kelly, V. Santiba nez and A. Loria

425

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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

It may be reduced to ) = q (t, q TKv q , q , q TTKv V q

TKv is symmetric positive denite (because, is a non-singular 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

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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 closed-loop equation is uniformly stable, and all the solutions are bounded, that is, Ln , q q , Lm .

R. Kelly, V. Santiba nez and A. Loria

427

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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

Ln , q Ln Thus, from q and q 2 , and Lemma A.5 we obtain

(t) = 0 IRn (control objective achieved). lim q

R. Kelly, V. Santiba nez and A. Loria

428

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

Example 16.3

Consider the Pelican robot

y g
Link 1

lc1

l1 x

I1 m1 q1 lc2 l2 m2 I2 q2
Link 2

Figure 104: Diagram of the Pelican robot.

R. Kelly, V. Santiba nez and A. Loria 429

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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 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
R. Kelly, V. Santiba nez and A. Loria 430

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

Parameterization of the dynamic model: M (q , )u + C (q , w , )v + g (q , ) = 11 12 13 1 = 2 21 22 23 3 +M0(q )u + C0(q , w)v + g 0(q ) where

2 u1 + l1g sin(q1) 11 = l1

12 = 2l1 cos(q2)u1 + l1 cos(q2)u2 l1 sin(q2)w2v1 l1 sin(q2)[w1 + w2]v2 + g sin(q1 + q2) 13 = u1 + u2

R. Kelly, V. Santiba nez and A. Loria 432

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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 .

R. Kelly, V. Santiba nez and A. Loria

433

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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

R. Kelly, V. Santiba nez and A. Loria

434

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

The control law becomes 11 + + Kv q = Kpq 21 12 22 13 23 1 2 3

2 (m1lc 1 + I1 )u1 + m1 lc1 g sin(q1 ) , + 0 0

while the adaptation law is 11[v1 q 1] 1] + 22[v2 q 2] ds + 12[v1 q 13[v1 q 1] + 23[v2 q 2]

1(t) = 2 ( t) 3(t)

t 0

1(0) . 2(0) 3(0)

435

R. Kelly, V. Santiba nez and A. Loria

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

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 .

R. Kelly, V. Santiba nez and A. Loria

436

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

0.02 0.01 0.00

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.

R. Kelly, V. Santiba nez and A. Loria

437

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

3.0 2.5 2.0 1.5 1.0 0.5 0.0

. . ........ .... ......... ....... ..... ......... . . . ..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 .

R. Kelly, V. Santiba nez and A. Loria

438

Control of Robot Manipulators in Joint Space

Ch. 16. PD Control with Adaptive Compensation

Thanks a lot for your attention!

R. Kelly, V. Santiba nez and A. Loria

439

http://www.springer.com/978-1-85233-994-4

Applied Control of Manipulation Robots
100% (1)
Applied Control of Manipulation Robots
487 pages
Swarm Robotics: Fundamentals and Applications
From Everand
Swarm Robotics: Fundamentals and Applications
Fouad Sabry
No ratings yet
Robotic End Effector Notes
No ratings yet
Robotic End Effector Notes
24 pages
Industrial Robotics PDF
71% (14)
Industrial Robotics PDF
39 pages
2012-1811. Robot Arm Kinematics DH Intro
No ratings yet
2012-1811. Robot Arm Kinematics DH Intro
75 pages
An Introduction To Robotic Manipulators
No ratings yet
An Introduction To Robotic Manipulators
29 pages
Robotics Programming and Applications
No ratings yet
Robotics Programming and Applications
38 pages
Jacobians-Differential Kinematics
No ratings yet
Jacobians-Differential Kinematics
62 pages
Introduction To Robotics: A Common View: Robots As Humanoids
No ratings yet
Introduction To Robotics: A Common View: Robots As Humanoids
24 pages
Robotics Book
No ratings yet
Robotics Book
137 pages
6 Robot Kinematics and Dynamics 1 PDF
No ratings yet
6 Robot Kinematics and Dynamics 1 PDF
45 pages
Sudharsan Engineering College: UNIT I Fundamentals of Robots
No ratings yet
Sudharsan Engineering College: UNIT I Fundamentals of Robots
7 pages
9-John J. Craig, "Introduction To Robotics Mechanics and Control", Third Edition, 2005
No ratings yet
9-John J. Craig, "Introduction To Robotics Mechanics and Control", Third Edition, 2005
41 pages
IAE Robotics Course Description
No ratings yet
IAE Robotics Course Description
6 pages
Industrial Robotics - Model Papers
No ratings yet
Industrial Robotics - Model Papers
3 pages
Robot Programming: by 143-Akash Sanghvi 144-Varun Sharma 145-Gopesh Raval 146-Nitesh Dawani
No ratings yet
Robot Programming: by 143-Akash Sanghvi 144-Varun Sharma 145-Gopesh Raval 146-Nitesh Dawani
10 pages
Industrial Robotics
No ratings yet
Industrial Robotics
30 pages
Lec8 Inverse Kinematics
No ratings yet
Lec8 Inverse Kinematics
32 pages
Experiment - 05: FIG1: 2 Prismatic Joint Manipulator FIG 2: 2 Prismatic Joint Manipulator
No ratings yet
Experiment - 05: FIG1: 2 Prismatic Joint Manipulator FIG 2: 2 Prismatic Joint Manipulator
8 pages
5 Mobile Robot Manueverability
No ratings yet
5 Mobile Robot Manueverability
12 pages
Robot Kinematics Powerpoint PDF
No ratings yet
Robot Kinematics Powerpoint PDF
45 pages
Unit 6 Industrial Robotics
No ratings yet
Unit 6 Industrial Robotics
74 pages
ROBOTICSQUESTION BANK Unit V, Vi, Vii, Viii by Mahendra Babu Mekala, PBRVITS, Kavali
100% (2)
ROBOTICSQUESTION BANK Unit V, Vi, Vii, Viii by Mahendra Babu Mekala, PBRVITS, Kavali
10 pages
OIE 751 ROBOTICS Unit 4 Class 5 (20-10-2020)
No ratings yet
OIE 751 ROBOTICS Unit 4 Class 5 (20-10-2020)
13 pages
VAL Programming
No ratings yet
VAL Programming
1 page
81 Part1 PathPlanning
No ratings yet
81 Part1 PathPlanning
35 pages
Principles of Robot Motion Theory, Algorithms, and Implementations PDF
No ratings yet
Principles of Robot Motion Theory, Algorithms, and Implementations PDF
616 pages
Forward and Reverse Kinematics
No ratings yet
Forward and Reverse Kinematics
3 pages
Advanced Robotics PDF
No ratings yet
Advanced Robotics PDF
47 pages
IESLCoE Robotics-Assignment - 2
No ratings yet
IESLCoE Robotics-Assignment - 2
11 pages
Robotics Kinematics and Dynamics
No ratings yet
Robotics Kinematics and Dynamics
24 pages
A Motion Control
No ratings yet
A Motion Control
6 pages
Roboticsbasic
No ratings yet
Roboticsbasic
52 pages
72 Part2 RobotDynamics
No ratings yet
72 Part2 RobotDynamics
30 pages
D-H Problems PDF
No ratings yet
D-H Problems PDF
9 pages
Robotics Kinematics and Dynamics
No ratings yet
Robotics Kinematics and Dynamics
6 pages
Robotics 2mark
No ratings yet
Robotics 2mark
14 pages
080 Manipulator Trajectory Planning
No ratings yet
080 Manipulator Trajectory Planning
23 pages
2 Fundamental Industrial Robot
No ratings yet
2 Fundamental Industrial Robot
83 pages
00 Robotics Lec00
No ratings yet
00 Robotics Lec00
14 pages
Lec02 Manipulators
No ratings yet
Lec02 Manipulators
7 pages
Industrial Robotics
No ratings yet
Industrial Robotics
28 pages
Parallel Manipulators - Edited by Jee Hwan Ryu PDF
No ratings yet
Parallel Manipulators - Edited by Jee Hwan Ryu PDF
508 pages
FFR125 LectureNotes
100% (2)
FFR125 LectureNotes
122 pages
Robot Motion Planning: Approaches and Research Issues
No ratings yet
Robot Motion Planning: Approaches and Research Issues
26 pages
Robotics Programming
No ratings yet
Robotics Programming
32 pages
Unit I Fundamentals of Robot: (Revision)
No ratings yet
Unit I Fundamentals of Robot: (Revision)
16 pages
Robotics
No ratings yet
Robotics
37 pages
WheeledKinematicsusing Matlab
No ratings yet
WheeledKinematicsusing Matlab
4 pages
Robot Trajectory Planning
No ratings yet
Robot Trajectory Planning
15 pages
Design and Manufacturing of Low Cost Scara Robot IJERTV10IS060027-With-cover-page-V2 3
No ratings yet
Design and Manufacturing of Low Cost Scara Robot IJERTV10IS060027-With-cover-page-V2 3
5 pages
Important Questions and Answers - Robot Drive Systems and End Effectors PDF
No ratings yet
Important Questions and Answers - Robot Drive Systems and End Effectors PDF
5 pages
Jacobian and Static Force
No ratings yet
Jacobian and Static Force
35 pages
Control
No ratings yet
Control
441 pages
Ch 1 Robotics Introduction 1st
No ratings yet
Ch 1 Robotics Introduction 1st
31 pages
2 Marks Question & Answer
No ratings yet
2 Marks Question & Answer
9 pages
1.Robotics
No ratings yet
1.Robotics
15 pages
kolesnyk2017
No ratings yet
kolesnyk2017
3 pages
Robotics Unit-1 QB
No ratings yet
Robotics Unit-1 QB
5 pages
04 DOF and Motion
No ratings yet
04 DOF and Motion
55 pages
Stability Analysis of Linear Time-Varying Time-Delay Systems by Non-Qua
No ratings yet
Stability Analysis of Linear Time-Varying Time-Delay Systems by Non-Qua
9 pages
Binder 1
No ratings yet
Binder 1
8 pages
Fundamentals of Lyapunov Theory
No ratings yet
Fundamentals of Lyapunov Theory
9 pages
Stability and Stabilization of Hybrid Systems: Mikael Johansson
No ratings yet
Stability and Stabilization of Hybrid Systems: Mikael Johansson
51 pages
I JC Finite Time
No ratings yet
I JC Finite Time
12 pages
Control of Robot Manipulators
100% (1)
Control of Robot Manipulators
441 pages
Applied Nonlunear Control: Fundamentals of Lyapunov Theory
No ratings yet
Applied Nonlunear Control: Fundamentals of Lyapunov Theory
10 pages
Permatasari 2018 J. Phys. Conf. Ser. 1025 012086
No ratings yet
Permatasari 2018 J. Phys. Conf. Ser. 1025 012086
8 pages
Exercises in Nonlinear Control 2
No ratings yet
Exercises in Nonlinear Control 2
116 pages
Variable Gradient Method
No ratings yet
Variable Gradient Method
1 page
Outline: Lyapunov's Linearization Method
No ratings yet
Outline: Lyapunov's Linearization Method
8 pages
Lyapunov Function - Dynamical Systems
100% (1)
Lyapunov Function - Dynamical Systems
4 pages
Exercises in Nonlinear Control Systems
No ratings yet
Exercises in Nonlinear Control Systems
115 pages
09 Fall 501 Lyap
No ratings yet
09 Fall 501 Lyap
10 pages
EE3302
No ratings yet
EE3302
4 pages
Chapter 4
No ratings yet
Chapter 4
2 pages
Ele4202 Control Systems Eng Ii Exam - Final - 2021
No ratings yet
Ele4202 Control Systems Eng Ii Exam - Final - 2021
3 pages
F X DT D: 1) Krasovskii's Theorem
No ratings yet
F X DT D: 1) Krasovskii's Theorem
51 pages
2 Lyapunov Stability
No ratings yet
2 Lyapunov Stability
6 pages
NCS21 - 04 - Lyapunov Stability - 01
No ratings yet
NCS21 - 04 - Lyapunov Stability - 01
50 pages
A Modal Approach To Induction Motor Dynamics.
100% (1)
A Modal Approach To Induction Motor Dynamics.
260 pages
TAM_AC
No ratings yet
TAM_AC
4 pages
A Review of Fundamentals of Lyapunov Theory
No ratings yet
A Review of Fundamentals of Lyapunov Theory
8 pages
Lecture 6: Stability of Dynamic Systems Lyapunov's Direct Method 6.1 Notions of Stability
No ratings yet
Lecture 6: Stability of Dynamic Systems Lyapunov's Direct Method 6.1 Notions of Stability
10 pages
MATH F312Tut 8 2024
No ratings yet
MATH F312Tut 8 2024
2 pages
Compendium
No ratings yet
Compendium
116 pages