chapter 7 manipulator controlcs336.stanford.edu/slides/cs223a_course_reader_2019... ·...

Chapter 7

Manipulator Control

7.1 Introduction

This chapter starts with a review of the basics of PID controllers. That is followedby a presentation of the general control structure for dynamic decoupling and con-trol of joint motions. The discussion is completed with a presentation of the basicsof the task-oriented operational space control, which provides dynamic decouplingand direct control of end-e!ector motions.

Consider the task of controlling the motion of an n-DOF manipulator for somegoal configuration defined by a set of desired joint positions. This task can beaccomplished by selecting n independent proportional-derivative, PD, controllersthat a!ect each joint to move from its current position to the goal position. Eachof these controllers can be viewed as a spring-damper system attached to thejoint. The spring’s neutral position corresponds to the goal position of the joint,as illustrated in Figure 7.1. Any disturbance from that position would result in arestoring force that moves the joint back to its goal position. During motion, thedamper contributes to the stability of the system.

These simple controllers are widely used in industrial robots to execute point-to-point motion tasks. However, these controllers are limited in their ability toperform motion tracking or any task that involves interaction with the environ-ment. In motion, the manipulator is subjected to the dynamic forces acting on itslinks. By ignoring these forces, PD controllers are limited in their performance forthese tasks. The control structures needed to address the dynamics of manipulatorsystems are presented in section 7.6.2. The discussion here focuses on independentPD controllers.

195

196 CHAPTER 7. MANIPULATOR CONTROL

Figure 7.1: Manipulator with springs and dampers.

When the goal position is specified in terms of the end-e!ector configuration, themanipulator can be directly controlled at the end e!ector. Imagine placing a 3Dspring-damper system at the end-e!ector itself, instead of the one placed at thejoints. In that case the end-e!ector will be attracted to move to its goal positionby the sti!ness of the spring, and the stability of the motion will be provided bythe damper.

A more general way to think about this approach is to imagine the applicationof a force at the end-e!ector, as the gradient of some attractive potential field,whose minimum is at the goal position. In that case thers is a need to add somedamping proportional to the velocity to stabilize the system at the goal position.To produce this type of control, it is necessary to be able to create a force at theend-e!ector, which must be produced by the actuators at the joints. This canbe accomplished with the transpose of the Jacobian, which relates forces at theend-e!ector to corresponding torques at the joints. This allows direct control ofthe e!ector without requiring any inverse kinematics.

The di"culty in practice with such controllers is their limited performance for mo-tion control, since the dynamics of the manipulator are ignored. The incorporationof the dynamics for the end-e!ector control is discussed in section 7.7.1.

Control with Inverse Kinematics Typically robot control has the followingstructure

Let xd be the desired position and orientation of the end e!ector. The inversekinematics are used to find the corresponding desired joint position qd. This is a

7.1. INTRODUCTION 197

Figure 7.2: Potential Field for a manipulator.

vector of desired positions for each joint that is transmitted to a set of indepen-dent controllers each of which is trying to minimize the error between the desiredjoint position and actual joint position. These controllers are simple PD or PIDcontrollers. Because of the computational complexity of inverse kinematics, thisapproach is di"cult to use for tasks involving real-time modifications of the end-e!ector desired position and orientation.

Control with Linearized Kinematics Another approach to the task trans-formation problem relies on the linearized kinematics and the use of the Jacobianand its inverse. This approach called resolved motion rate control was first pro-posed by Whitney in 1972.

To a small joint space displacement !q corresponds a small end-e!ector displace-ment !x. Given !q, the corresponding displacement !x is given by

!x = J(q)!q (7.1)

When it exists, the inverse of the Jacobian allows the computation of the displace-ment !q that corresponds to a desired displacement !x,

!q = J!1(q)!x (7.2)

If the manipulator task consisted of following a path of the end e!ector, thisrelationship can be used to continuously increment the joint position in accordancewith small displacements along the end-e!ector path. At a given configuration q,


Figure 7.3: Inverse kinematics robot control.

the end-e!ector position and orientation is determined by the forward kinematics,x = f(q). Selecting a neighboring desired end-e!ector configuration xd results ina small end-e!ector displacement.

!x = xd ! x

The corresponding joint displacement !q can be established using the inverse ofthe Jacobian matrix.

!q = J!1(q)!x

From the current joint configuration q, this allows to compute the desired jointconfiguration qd as,

qd = q+ !q

In general, the task transformation problem involves, in addition, transformationsof the end-e!ector desired velocities and accelerations into joint descriptions. Tasktransformations are computationally demanding and are di"cult to carry out inreal-time.

7.2. PASSIVE NATURAL SYSTEMS 199

-

-

-

-

Figure 7.4: Linearized kinematics robot control.

7.2 Passive Natural Systems

The behavior of a PD controlled mechanism has common characteristics with pas-sive spring-damper systems. This section considers the mass-spring-damper systemshown in Figure 7.5. The study of this natural 1 DOF system will provide the basisfor the development of PD controllers.

Figure 7.5: Spring-mass system.

7.2.1 Conservative Systems

Consider a mass m connected to a spring of sti!ness k. The position of the mass isdetermined by the coordinate x, and the neutral position of the spring is assumedat x = 0. The kinetic energy of this system is

K =1

2mx2


The potential energy of this system is due to the spring. The mass is subjected tothe force

f = !kx

which is the gradient of the spring potential energy

V =1

2kx2

The Lagrangian equation for this system is

d

dt("L

"x)!

"L

"x= 0 (7.3)

This system is conservative, since the only force acting on it is a conservative forcedue to a potential energy. On the right hand side of Lagrange’s equation, theexternal force is zero. The total energy of the system is therefore constant. Thusthis system is stable, but oscillatory. The equation of motion is

mx+ kx = 0 (7.4)

The potential energy of the system can be set initially by pulling on the mass.The system will start with zero kinetic energy, its velocity will increase and thepotential energy will be transfered into kinetic energy. At the neutral position, thepotential energy becomes zero and the kinetic energy starts to transfer back intopotential energy.

As illustrated in Figure 7.6, the potential is set to some level. After its release,the mass oscillates between two positions with a frequency that depends on bothk and m – higher frequency with higher sti!ness and smaller mass. The natural

frequency, #n of this system is

#n =

!k

m(7.5)

The equation of motion can be written in the form

x+ #2nx = 0 (7.6)

and the time response, x(t), of this system is


Figure 7.6: System’s response.

x(t) = c cos(#nt+ $) (7.7)

where c and $ are constants depending on the initial conditions.

7.2.2 Dissipative Systems

In a real setting there is always some amount of friction acting on a mechanicalsystem. Assume that the friction acting on the mass-spring is simply viscous,

ffriction = !bx

With the friction, the Lagrange equation is

d

dt("L

"x)!

"L

"x= ffriction (7.8)


The dissipative force ffriction appears now as an external force on the right handside of the equation. This system is dissipative, and is described by the secondorder equation

mx+ bx+ kx = 0 (7.9)

To analyze the characteristics of this system, divide the equation by m."

k/mrepresents the natural frequency of the system and b/m represents the damping ofthe system.

x+b

mx+

k

mx = 0 (7.10)

The friction results in an oscillatory-damped behavior. As the friction coe"cientb increases, the magnitude of oscillations decreases at a faster rate. If b was verylarge, the system will be over damped. It will never cross the zero axis, slowlymoving toward the goal position. Between these two states, there is a critically-

damped behavior of the system. As will be seen, this behavior is quite desirablein the development of control systems. The analysis of the time-response of thesystem shows that the critically-damped state is reached when

b

m= 2#n (7.11)

Figure 7.7: Dissipative Systems.


Treating the critically-damped state as a reference state, introduce a ratio describ-ing the damping b/m with respect to its critical value when b/m = 2#n. Thenatural damping ratio is defined as

%n =b

2#nm=

b

2"km

(7.12)

The system is critically damped when %n = 1, It’s over damped if %n > 1, andoscillatory when %n < 1. With #n and %n, the equation of motion is

x+ 2%n#nx+ #2nx = 0 (7.13)

The time-response of this system is

x(t) = ce!!n"nt cos(#n

"1! %2nt+ $) (7.14)

Figure 7.8: Dissipative Systems Response.

The exponential decreases with the damping ratio, and the frequency of oscillationis a!ected by #n

"1! %2n. This quantity is defined as the damped natural frequency,

# = #n

"1! %2n


When %n = 1, the system is critically damped and there are no oscillations. Thesmaller %n, the closer is the damped natural frequency is to the natural frequency.The period of the oscillations is 2!

"n

"1!!2n

.

Example Consider the simple example when m = 2.0; b = 4.8; k = 8.0. Since

# = #n

"1! %2n then #n =

#km = 2, %n = b

2"km

= 0.6 and # = 2."1! 0.36 = 1.6.

7.3 Passive-Behavior Control

The passive behavior of a natural system can be reproduced in the design of therobot controller. Consider a robot with 1-DOF prismatic joint, with mass m. Thetask is to move the robot from its current configuration, x0, to a desired positionxd. The actuation of this robot involves one force, f , acting on the prismatic link.The robot equation of motion is

mx = f

Figure 7.9: Simple goal control.

The goal is to find the force f that accomplishes the task, while providing passivebehavior. To achieve this behavior, select f to be conservative – the gradient of apositive potential function. For this task, the potential function will be designedto have a zero minimum at the desired position xd,

V (x) =

$0, x = xd

V (x) > 0, x #= xd

The simplest such function is the potential

7.3. PASSIVE-BEHAVIOR CONTROL 205

Figure 7.10: Potential function.

V (x) =1

2kp(x! xd)

2

The gradient of this potential is the conservative force

f = !$V (x) = !"V

"x

Applying this control to the robot, leads to

mx = !"

"x[1

2kp(x! xd)

2] (7.15)

The behavior of the controlled closed-loop system is

mx+ kp(x! xd) = 0 (7.16)

This behavior is identical to that already seen for the conservative mass-springsystem. In this controlled system, however, the natural springiness, is reproducedby the parameter kp, which controls the artificial sti!ness of the closed loop system.Under this control, the link will oscillate around the desired position xd, in similarfashion to the mass-spring system. The frequency of this oscillation is determinedby"kp/m. This frequency represents the closed loop frequency, # of the controlled

system.

With its conservative behavior, this system is stable. However, it is not asymp-totically stable. Asymptotic stability can be achieved by the application of a


dissipative force. This non-conservative force will appear on the right-hand side ofLagrange equation,

d

dt("L

"x)!

"L

"x= &dissipative (7.17)

The general condition for asymptotic stability is

xT &dissipative < 0, for x #= 0 (7.18)

Intuitively, the above condition implies that the dissipative forces are always actingto oppose the velocity. Choosing the dissipative force as fd = !kvx, the asymptoticstability condition for this system becomes

xT (!kvx) = !kvx2 < 0, for x #= 0 (7.19)

This condition is satisfied if kv > 0. The total control force becomes

f = !kp(x! xd)! kvx (7.20)

where kp > 0 and kv > 0. This is simply the conventional proportional-derivativecontrol. kp is position gain and kv is the velocity gain. The closed-loop systemcorresponding to this control is described by

mx+ kvx+ kpx = kpxd (7.21)

To determine the characteristics of this 2nd order system, divide the above equationby m and introduce the frequency and damping ratio, as proceeded with the mass-spring system. This leads to

x+ 2%#x+ #2x = #2xd (7.22)

with

#2 =kpm

(7.23)

% =kv

2"kpm

(7.24)


where # is the closed-loop frequency and % is the closed-loop damping ratio. Sincethese two parameters determine the response of the controlled system, first set #and % to achieve the desired response. The position gain kp and the velocity gainkv can be then selected in accordance with the desired behavior. These are

kp = m#2 (7.25)

kv = m(2%#) (7.26)

The above expressions for kp and kv show that both of these gains are proportionalto the mass m, given a selection of # and %. For a system with unit mass, thesegains would be given by

k#p = #2 (7.27)

k#v = 2%# (7.28)

k#p and k#v represent the position and velocity gains that correspond to a desireddynamic response for a unit-mass system.

1.x = f # with f # = !k#vx! k#p(x! xd)

and the closed-loop behavior for the single unit-mass system is

1.x+ k#vx+ k#px = k#pxd

Given the control gains for a desired behavior of a unit-mass system, the gainsthat provide the same behavior for an m-mass system are given by

kp = mk#pkv = mk#v

and the control f of the m-mass system is

mx = f with f = mf #

These relationships play an important role in extending the analysis to systemswith nonlinearities and larger numbers of degrees of freedom.


7.3.1 Nonlinear Systems

Consider again the 1-DOF prismatic arm. A more realistic model of this systemwill include some amount of friction, which will be approximated by a nonlinearfunction of the position and velocity, i.e. b(x, x). The system is now described bythe equation

mx+ b(x, x) = f (7.29)

If it was possible to model the friction b, this model could be used in the control ofthe system to compensate for this friction, and to control the resulting linearizedsystem as before. The general structure for implementing this type of control is

f = 'f # + ( (7.30)

where ( represents the portion of the control that compensates for the nonlinearforces acting on the system, and ' is the mass of the system allowing the use ofunit-mass system’s control design f #. Since both the mass and the nonlinearities inthe system must be identified, ' and ( will only be estimates of these quantities.In the case of our system, ' and ( are

' = %m (7.31)

( = %b(x, x) (7.32)

%m and %b(x, x) are the estimate for the mass and friction of the system.

mx+ b(x, x) = %mf # +%b(x, x) (7.33)

With perfect estimates, (%m = m, and %b = b), the closed-loop behavior of thissystem will be described by the the unit-mass system controlled by f #

1.x = f #

f #, the control input of the linearized unit-mass system, will be designed withk#p and k#v to achieve the desired behavior. This control structure is shown inFigure 7.11. The dotted block in this Figure represents the unit-mass systembeing controlled by f # with outputs x and x.


Figure 7.11: Non-linear control system.

7.3.2 Motion Control

The task discussed above concerned placing the robot at a desired position, xd.The system robot system

mx+ b(x, x) = f

is controlled by selecting

f = %mf # +%b(x, x)

with

f # = !k#vx! k#p(x! xd)

where k#p and k#v are the PD control gains. With perfect estimates, the closed-loopbehavior is described by

1.x+ k#vx+ k#p(x! xd) = 0 (7.34)

A robot task may involve the tracking of a desired trajectory xd(t). In additionto the time-varying desired position, a trajectory tracking task generally involvesthe desired velocities and accelerations, i.e. xd(t) and xd(t). The robot control forthis task will have an identical structure to the controller described above, with anew unit-mass control input, f #, designed for trajectory tracking


f # = xd ! k#v(x! xd)! k#p(x! xd) (7.35)

The closed-loop system is described by

(x! xd) + k#v(x! xd) + k#p(x! xd) = 0 (7.36)

This is a second order system in the error e = x! xd,

e+ k#v e+ k#pe = 0 (7.37)

Figure 7.12: Error correction control.

The trajectory tracking control system is shown in Figure 7.12. In addition toposition and velocity feedback, trajectory tracking control involves an accelerationfeed-forward component that allows the system to better anticipate changes on thedesired trajectory.

7.4 Disturbance Rejection

The linear closed-loop behavior we have achieved relies on the assumption of exactestimates of the system’s parameters. The errors in these estimates, in addition toother un-modeled aspects of the system, all contribute to disturbance forces thatcould a!ect the performance and stability of the system. The e!ects of these dis-turbances can be minimized by appropriate selection of gains, k#p and k#v, involvedin the unit-mass controller. The larger these gains are the better the disturbance

7.4. DISTURBANCE REJECTION 211

rejection of the system becomes. There are, however, various factors that limit thegains, as will be seen in section 7.4.1.

Let us assume that all disturbances acting on the system can be represented bya single disturbance force fdist, that is directly acting at the input of system, asillustrated in Figure 7.13. In addition, this disturbance force will be assumed tobe constant. The system’s equation of motion becomes

mx+ b(x, x) = f + fdist (7.38)

Figure 7.13: Disturbance rejection control.

Using the control structure

f = mf # + b(x, x)

and the unit-mass control for trajectory tracking

f # = xd ! k#v(x! xd)! k#p(x! xd)

The closed-loop behavior of the unit-mass system is

e+ k#v e+ k#pe =fdistm

(7.39)

For a desired position task,


f # = !k#vx! k#p(x! xd)

The closed-loop behavior of the system is

1.x+ k#vx+ k#p(x! xd) =fdistm

Steady-State Error The steady-state error is determined by analysis of theclosed-loop system at rest, i.e. when all derivatives are set to zero. This leads tothe steady-state equation

k#pe =fdistm

(7.40)

Then the steady-state error is

e =fdistmk#p

=fdistkp

(7.41)

Thus high values of k#p reduce the steady-state error. This also shows that heavysystems (with large masses) are less sensitive to disturbances.

Figure 7.14: One-DOF prismatic manipulator.

Example Let us consider the 1-DOF manipulator shown in Figure 7.14.

The manipulator is controlled to the desired position xd. The closed-loop behaviorof this system is

7.4. DISTURBANCE REJECTION 213

mx+ kvx+ kp(x! xd) = 0 (7.42)

Let us consider the application of a disturbance force, fdist, to this system, and letus find the new position of the manipulator. At rest, the steady-state equation is

kp(x! xd) = fdist

and the manipulator is positioned at

x = xd +fdistkp

7.4.1 Control Gain Limitations

The higher kp is, the better the disturbance rejection becomes. However, controlgains are limited by various factors involving structural flexibilities in the mecha-nism, time-delays in actuators and sensing, and sampling rates. An increase of kpresults in an increase of the closed-loop frequency #. As this frequency approachesthe the first unmodeled resonant frequency, #low!resonant, the corresponding modecan be excited. It is thus important to keep # well below this frequency. In ad-dition, # must be remain below the frequency corresponding to the largest timedelay, #large!delay. The frequency associated with the sampling rate, #sampling!rate

also imposes a limitation on #. Typically # is selected as

# <1

2#low!resonant

# <1

3#large!delay

# <1

5#sampling!rate

7.4.2 Integral Control

The addition of integral action to the PD controllers analyzed thus far, will allow tofurther reduce disturbance errors. For a trajectory tracking task, A PID controller(proportional-integral-derivative) involves, in addition to k#p, and k#v, the integralgain k#i. A PID controller for a trajectory tracking task is


f # = xd ! k#v(x! xd)! k#p(x! xd)! k#i

&(x! xd)dt (7.43)

The closed-loop behavior in the presence of a disturbance force is

e+ k#v e+ k#pe+ k#i

&edt =

fdistm

(7.44)

The disturbance force is assumed constant. Taking the the derivative of the equa-tion above yields

···e +k#v e+ k#pe+ k#ie = 0 (7.45)

The steady-state error equation (all derivatives set to zero) is

e = 0

7.5 Actuation System

Consider the following illustration of gain selection for e!ective inertia optimiza-tion. Figure 7.15 depicts a gear reduction system.

Figure 7.15: Gear reduction.

The gear ratio is ) = Rr , and the relationships for angles and torques at the motor

and links are

7.6. PD CONTROL FOR MULTI-LINK SYSTEMS 215

*L = (1

))*m (7.46)

&L = )&m (7.47)

The corresponding equations of motion are

&m = Im*m +1

)(IL*L) + bm*m +

1

)bL*L (7.48)

Since

*L =1

)*m

then

&m = (Im +IL)2

)*m + (bm +bL)2

)*m (7.49)

and

&L = (IL + )2Im)*L + (bL + )2bm)*L (7.50)

(IL+)2Im) represents the e!ective inertia, and (bL+)2bm) represents the e!ectivedamping both perceived at the link. Finally, the position and velocity gains canbe selected as

kp = (IL + )2Im)k#p (7.51)

kv = (IL + )2Im)k#v (7.52)

7.6 PD Control for Multi-Link Systems

The control of multi-link manipulator system can be accomplished with a set ofPD controllers designed independently for each link. While su"cient for pick-and-place tasks, this type of control is limited in its performance for tasks involving


Figure 7.16: Two-DOF manipulator example.

trajectory tracking and interactions with the environment. To analyze the limi-tations of PD controllers, consider the example of two revolute-joint manipulatorshown Figure 7.16.

The dynamic equation of motion for this manipulator is

'm11 m12

m21 m22

('*1*2

(+

'm112

0

()*1*2

*+

'0 m122

!m1122 0

('*21*22

(+

'g1g2

(=

'&1&2

(

(7.53)

The two scalar equations corresponding to the behavior of joint 1 and joint 2 are

m11*1 +m12*2 +m112*1*2 +m122*22 +G1 = &1 (7.54)

m22*2 +m21*1 !m112

2*21 +G2 = &2 (7.55)

In the design of two independent PD controllers for this robot, the dynamic cou-pling between the two links is ignored, and these links are treated as two decoupledsystems described by

m11*1 = &1 (7.56)

m22*2 = &2 (7.57)

These equations neglect the dynamic forces acting on the joints, and ignore theconfiguration dependency of the link inertias. The actual system is nonlinear and


Figure 7.17: Controller for Two-DOF example.

highly coupled, as illustrated in Figure 7.17. The dynamic disturbances acting onan n-DOF manipulator controlled by n independent PD systems is illustrated inFigure 7.18.

7.6.1 Stability of PD Control

The dynamics of an n DOF manipulator are described by

M q+B(q)[qq] + C(q)[q2] + g(q) = & (7.58)

With PD control, the joint torques are selected as

& = !kp(q! qd)! kvq (7.59)


Figure 7.18: Coupled n-DOF control.

The stability of this system can be easily concluded, as all external forces actingon this system are either conservative (!kp(q ! qd), gradient of a potential) ordissipative (!kvq). That is

& = !$qVd ! kvq (7.60)

where

(Vd =1

2kp(q! qd)

T (q! qd)


To further analyze the stability, consider again Lagrange’s equation, from whichequation 7.58 was obtained.

d

dt("K

"q)!

"(K ! Vgravity)

"q= !$Vd ! kvq (7.61)

where Vgravity represents the system’s natural potential energy due to the gravity.Applying the control torque of equation 7.60, the controlled system becomes

d

dt("K

"q)!

"(K ! (Vgravity + Vd))

"q= &dissipative (7.62)

where

&dissipative = !kvq

This shows how the conservative portion of the control modifies the potential en-ergy (Vgravity to Vgravity+Vd). Without dissipative forces, this system is oscillatory,but stable. The addition of dissipative force provides asymptotic stability, underthe condition

qT &dissipative < 0

which is verified for the dissipative force !kvq if kv > 0.

A manipulator controlled with a set of independent PD controllers is stable, asthe e!ect of these controllers is only to modify the manipulator’s potential energy,while providing the dissipation needed for asymptotic stability.

7.6.2 Joint Space Dynamic Control

While providing stability, a PD controller is limited in its performance as it ignoresthe dynamic coupling forces. High gains provide better disturbance rejection, butas mentioned earlier, control gains are limited by the system’s flexibilities, timedelays and sampling rate. Dynamic decoupling and motion control of the robotsystem can be accomplished by a control structure that uses the manipulatordynamic model. The manipulator dynamics are described by

M(q)q+ v(q, q) + g(q) = & (7.63)


Based on this model, the control structure for dynamic decoupling and control is

& = +M(q)& # + %v(q, q) + %g(q) (7.64)

where %. represents an estimate. Appling this control to the robot, the closed-loopbehavior will be described by

1.q = (M!1+M)& # +M!1[(%v ! v) + (%g ! g)] (7.65)

With perfect estimates, the system is described by the unit-mass system

1.q = & # (7.66)

With a PD design, the control input for the unit-mass systems, & #, is

& # = qd ! k#v(q! qd)! k#p(q! qd) (7.67)

and the closed-loop system is

e+ k#ve+ k#pe = 0 (7.68)

with

e = q! qd

The overall control system is shown in Figure 7.19. This structure provides de-coupling and linearization of the robot system, rendering it as set of unit-masssystems, controlled by & #. The control input to the decoupled system was selectedas a set of simple PD controllers, but obviously other control designs can be used.

The input to the control system shown in Figure 7.19 is the joint trajectory, qd,qd and qd. However, robot tasks are generally specified in terms of end-e!ectordescriptions. In which case, the end-e!ector task must be first transformed intojoint specifications.

7.7. OPERATIONAL SPACE CONTROL 221

Figure 7.19: Closed system diagram.

7.7 Operational Space Control

The operational space framework provides direct control of the end-e!ector mo-tions, eliminating the need for task transformation. The problem of task trans-formation and joint coordination is yet more di"cult for manipulation involvingredundant mechanisms or multiple robots. The manipulation of an object, forexample, by two robots, as illustrated in Figure 7.20, requires complex real-timecoordination. This becomes unnecessary with the direct control of the manipulatedobject provided in the operational space approach.

Figure 7.20: Multiple arm manipulation.

The basic idea in the operational space approach is to control the end e!ector by


a potential function whose minimum is at the end-e!ector goal position.

Vgoal(x) =1

2kp(x! xgoal)

T (x! xgoal) (7.69)

The corresponding force, F, that must be created at the end is given by the gradientof this potential,

F = !$xVgoal(x)

This force will be produced by a torque vector at the joint of the robot, givensimply by

& = JTF

Other more complex behaviors can be simply created by the design of artificialpotential functions to avoid joint limits, kinematic singularities, or obstacles.

The Lagrange’s equation can be used to analyze the stability of this type of control:

d

dt("K

"q)!

"(K ! Vgravity)

"q= & (7.70)

The control forces applied to the system are

& = JT (!$xVgoal) (7.71)

which can be rewritten as

& = !$qVgoal (7.72)

and the controlled system becomes

d

dt("K

"q)!

"(K ! (Vgravity + Vgoal))

"q= 0 (7.73)

This system is stable. The asymptotic stability requires the addition of dampingforces, for instance

Fd = !kvx

7.7. OPERATIONAL SPACE CONTROL 223

The corresponding torques are

&d = JT (!kvx)

and the Lagrange’s equation becomes

d

dt("K

"q)!

"(K ! (Vgravity + Vgoal))

"q= &d

The condition for asymptotic stability is

qT &d < 0 (7.74)

Replacing x by J q yields

qT &d = !kv[qT (JTJ)q)] < 0

For a non-redundant manipulator and outside of singularities, JTJ , is a positivedefinite matrix, and the quantity [qT (JTJ)q)] is positive. The system is thenasymptotically stable if kv > 0.

7.7.1 Operational Space Dynamics

The description of the dynamics at the end-e!ector requires first to select a set ofgeneralized coordinates, x, that represent the end-e!ector position and orientation,e.g. (x, y, z,',(, +). The kinetic energy of the system can then be expressed as aquadratic form of the generalized velocities, x,

Kx =1

2xTMxx (7.75)

where Mx represents the mass matrix associated with the inertial properties atthe end-e!ector. Let F be the vector of generalized forces corresponding to thegeneralized coordinates x. The end-e!ector equations of motion are

d

dt("K

"x)!

"(K ! V )

"x= F (7.76)

which can be developed in the form


Mxx+ vx(q, q) + gx(q) = F

This equation is similar to the one obtained for joint space dynamics. In fact,joint-space and operational-space dynamics are related by simple relationships.First examine the kinetic energy. In terms of joint velocities, the kinetic energy ofthe system is

Kq =1

2qTM q (7.77)

where M is the joint space mass matrix. Expressing the fact that Kx = Kq

establishes the relationship

JTMxJ = M (7.78)

which leads to

Mx = J!TMJ!1 (7.79)

The relationship between the gravity force vectors g and gx is simply given by thetranspose of the Jacobian matrix.

g = JTgx

The relationship between v and vx involves the time derivatives of the Jacobianmatrix (x = J q+ J q). In summary these relationships are

Mx = J!TMJ!1 (7.80)

vx = J!Tv !MxJ q (7.81)

gx = J!Tg (7.82)

7.7.2 Operational Space Dynamic Control

The end-e!ector dynamics is described by the equation

Mxx+ vx(q, q) + gx(q) = F

7.8. EXERCISES 225

Figure 7.21: Operational Space diagram.

The control structure for dynamic decoupling and motion control is

F = +MxF# + %vx + %gx (7.83)

where %. represents an estimate. F# is the input of the unit-mass system,

1.x = F# (7.84)

For an end-e!ector trajectory following, F# is

F# = xd ! kv(x! xd)! kp(x! xd) (7.85)

The operational space control structure is shown in Figure 7.21. Here, the trajec-tory is directly specified in terms of end-e!ector motion, xd, xd and xd.

7.8 Exercises

Exercise 7.1. Consider the planar manipulator in Figure 7.16 of the text. Thecorresponding equations of motion are given in section 7.6. Let m1 = 2, m2 = 1,and l1 = l2 = 1.0.


(a) Assume the manipulator is moving the horizontal plane, so G(q) = ,0. If

q =,0.2618 0.5236

-Tradians, q = ,0, and q =

,0.0 10.0

-T radsec2 , what

is the inertial coupling torque seen at joint 1?

(b) With the manipulator still in the horizontal plane, assume that the joints

are moving at constant velocity: q =,1.5 !1.0

-T radsec , q = ,0. At some

point during the motion, the configuration is the same as in part (a). Whatis the disturbance torque at each joint due to centrifugal/coriolis forces atthat instant?

(c) Finally, return the manipulator to the vertical plane so that gravity forcesare present. We will now apply motor torques which correspond to the PDcontroller:

& = !.100 00 100

/(q ! qd)!

.10 00 10

/q

Once the manipulator has come to rest using this control law with somedesired position, the actual position reading obtained from the joint posi-

tion sensors is q =,0.2618 0.5236

-Tradians. Treating the gravity as a

disturbance force at this configuration, what is the steady state error?

(d) What was the desired position of the manipulator from part (c)?

Exercise 7.2. The equations of motion for the RR planer manipulator in Figure7.22 is given as,.&1&2

/=

.4 + 2C2 1 + C2

1 + C2 1

/ .*1*2

/+

.!S2(*22 + 2*1*2)

S2*21

/+

.(C12 + 3C1)g

C12g

/,

where the gravity is acting in the negative Y0 direction.

The system is controlled with the following PD controller which uses a non-linearmodel-based portion with goal position (qd = qd = 0) to critically damp the systemat all times.

& = '& # + (

& # = !.K #

V10

0 K #V2

/q !

.K #

P10

0 K #P2

/(q ! qd)

(a) Find ' and (.

7.8. EXERCISES 227

X0

Y0

θ1

θ2

Figure 7.22: (Exercise 7.2) RR planar manipulator.

(b) Find the minimum K #P2

such that it is guaranteed to have the steady-stateerror, e2 is less than or equal to 0.5% of the disturbance force, &dist2 in anyconfiguration. Also, find the maximum steady-state error, e1 in terms of&dist1 when K #

P1= 400. Assume &dist1 = 2&dist2 .

(c) Find corresponding K #V1

and K #V2.

(d) Find #1, #2, %1, and %2 by using results from (b) and (c).

(e) Finally, place the manipulator to the frictionless horizontal plane so thatits XoYo plane is on a frictionless table and its Zo axis is pointing upwardfrom the table. In other words, all links of the manipulator are lying on thetable with their joint axes are vertical to the table surface. Since there is nofriction, the manipulator is now floating on the table and the gravity is nowacting on the negative Zo direction. Notice that the gravity has no e!ect onthe manipulator in this setup since all links are supported by the frictionlesstable, i.e., G(q) = ,0. Once the manipulator has come to rest using thiscontrol law with some desired position, the actual position reading obtainedfrom the joint position sensors is q = [0$ 90$]T . Treating the “no gravity”as a disturbance force at this configuration, what is the steady-state error ifg = 9.81m/s2?

(f) What was the desired position of the manipulator from part (e)?


Exercise 7.3. Consider the 1-DOF system with equation of motion:

f = ml2* + v* +mlg cos(*)

We are using a control strategy which compensates for the non-linear part of thesystem and has a unit-mass linear controller for trajectory tracking:

f = 'f # + (

' = ml2

( = v* + mlg cos(*)

f # = *d ! k#v(* ! *d)! k#p(* ! *d)

(where m is the estimate of the mass m of our system.)

If there is an error in our mass estimate, given by - = m ! m, then what is theresultant steady-state position error of the controlled system? Assume positionerror is given by e = * ! *d. Your answer should be in terms of -, m, l, k#p, *d, *,and g.

Exercise 7.4. Consider the 2-link RP manipulator shown in Figure 7.23: Itsequations of motion were derived in the text and are shown here:

&1 = (m1L21 + Izz1 + Izz2 +m2d

22) *1 + 2m2d2*1d2 + (m1L1 +m2d2)g cos(*1)

&2 = m2d2 !m2d2*12+m2g sin(*1)

The manipulator parameters have the following numerical values: L1 = 0.2m,m1 = 1.0kg, m2 = 0.8kg, Izz1 = 0.1kgm2, Izz2 = 0.07kgm2, and the range of d2 isbetween 0.5m and 1.0m.

(a) The system is controlled by a joint-space dynamic decoupling control, & ='& # + (, which compensates the non-linear part of the system, decouplesthe dynamics, and tracks a desired trajectory (ie. position, velocity andacceleration) separately for each joint. Leaving only the feedback gains (k#p1,k#p2, k

#v1, k

#v2) as symbols, give values for the matrix ', vector (, and vectors

& and & #. Note: you should also leave the joint variables (*1, d2) and jointvelocities (*1, d2) as symbols.

(b) Find the values for the gains k#p1, k#p2, k#v1, k#v2 such that the closed-loop

system for joint 1 is critically damped with natural frequency of 20 rad/sec,and the closed-loop system for joint 2 is critically damped with naturalfrequency of 25 rad/sec.

7.8. EXERCISES 229

Figure 7.23: (Exercise 7.4 (a)-(c)) RP manipulator.

(c) Consider the original equations of motion (ie. without a controller), whend2 = 0.6m. For joint 1, what is the e!ective inertia “seen” by the joint if wehave gearing with ratio ) = 5 and motor inertia Im = 0.004kgm2?

Consider again the original system in Figure 7.24 (ie. no controller or gearing).You are given DH coordinate frames as shown below: The length from the center

Figure 7.24: (Exercise 7.4 (d)-(e)) RP manipulator.

of mass of link 2 to the end-e!ector is L3. In this case, the end-e!ector positionin the plane is:


0Pend =

.s1(d2 + L3)!c1(d2 + L3)

/

(d) Use 0Pend to compute the Jacobian for linear velocity at the end-e!ector inframe {0}.

(e) Using your answer from part (d), the configuration *1 = 45$, d2 = 0.6m, andassuming L3 = 0.2m, compute the system’s mass matrix Mx in frame {0}when the dynamics are written in terms of the operational space coordinates.

Exercise 7.5. Consider the 1-DOF system described the equation of motion,4x+ 20x+ 25x = f .

(a) Find the natural frequency #n and the natural damping ratio .n of thenatural (passive) system (f = 0). What type of system is this (oscillatory,overdamped, etc.) ?

(b) Design a PD controller that achieves critical damping with a closed-loopsti!ness kCL = 36. In other words, let f = !kvx ! kpx, and determine thegains kv and kp. Assume that the desired position is xd = 0.

(c) Assume that the friction model changes from linear (20x) to Coulomb fric-tion, 30sign(x). Design a control system which uses a non-linear model-based portion with trajectory following to critically damp the system at alltimes and maintain a closed-loop sti!ness of kCL = 36. In other words, letf = 'f # + ( and f # = xd ! k#v(x! xd)! k#p(x! xd). Then, find f ,',(,f #,k#pand k#v. Note that f is an m-mass control, and f # is a unit-mass control.Use the definition of error, e = x! xd.

(d) Given a disturbance force fdist = 4, what is the steady-state (e = e = 0)error of the system in part (c)?

Exercise 7.6. For a certain RR manipulator, the equations of motion are givenby .

4 + c2 1 + c21 + c2 1

/ .*1*2

/+

.!s2(*22 + 2*1*2)

s2*21

/=

.&1&2

/

(a) Assume that joint 2 is locked at some value *2 using brakes and joint 1 iscontrolled with a PD controller, &1 = !40*1 ! 400(*1 ! *1d). What is theminimum and maximum inertia perceived at joint 1 as we vary *2? Whatare the corresponding closed-loop frequencies?

7.8. EXERCISES 231

(b) Still assuming that joint 2 is locked, at what values of *2 do the minimumand maximum damping ratios occur?

(c) Now assume that both joints are free to move, and that this system is con-trolled by a partitioned PD controller, & = '& # + (. Design a partitioned,trajectory-following controller (one that tracks a desired position, velocityand acceleration) which will provide a closed-loop frequency of 10 rad/secon joint 1 and 20 rad/sec on joint 2 and be critically damped over the entireworkspace. That is, let

& # = *d !.k#v1 00 k#v2

/(* ! *d)!

.k#p1 00 k#p2

/(* ! *d)

then find the matrices ' and ( and the vector & , along with the necessarygains k#vi and k#pi .

(d) If *2 = 180$, what is the steady-state error vector for a given disturbancetorque, &dist = [2 4]T ?

Exercise 7.7. Consider the mass-spring system in Fig. 7.25:

m = 2 kg

k = 1 N/m

b = 3 N.s/m

Figure 7.25: (Exercise 7.7) Mass-spring system.

If perturbed from rest, are the motions of the block under-damped, over-dampedor critically damped?


Exercise 7.8. Design a trajectory following controller with the structure shownin Fig. 7.26 for a system with dynamics

f = Ax2xx+Bx2 + C sinx

such that errors are suppressed in a critically damped fashion over all configurationsand the closed loop sti!ness is KCL. (Give expressions for Kp,Kv,',().

Figure 7.26: (Exercise 7.8) PPP manipulator.

Exercise 7.9. For the system shown in Fig. 7.27 the equations of motion aregiven by

Figure 7.27: (Exercise 7.9) RR planar manipulator.

7.8. EXERCISES 233

&1 =m2l22(*1 + *2 +m2l1l2c2(2*1 + *2) + (m1 +m2)l

21*1 !m2l1l2s2*

22

! 2m2l1l2s2*1*2 +m2l2gc12 + (m1 +m2)l1gc1,

&2 =m2l2c2*1 +m2l1l2s2*21 +m2l2gc12 +m2l

22(*1 + *2).

Using the symbols in these equations, give explicit equations for computing thevalues of &2 for a joint based controller which will. Use a model based controlsystem as given by & = '& # + ( to produce decoupled control with a servo law ofthe form & # = *d +KvE +KpE with

Kp =

.100 00 110

/and Kv =

.29 00 35

/

Give your results for the case where the joint angles and trajectories of each jointare respectively the straight line

*1 = 0.1 + 0.4t and *2 = 0.6t

Exercise 7.10. Using our methodology, design a control system (i.e. give ',(,Kp,Kv)for the following 2-DOF system:

m1x1 + k1x1 ! k1x2 = F1

m2x2 + (k1 + k2)x2 ! k1x1 = F2

We desire critically damped response to errors and a closed loop sti!ness of KCL =100 for both degrees of freedom.

Exercise 7.11. In order to implement a hybrid controller of the type in diagram7.28 (including the omitted velocity loop), if the manipulator has 6 degree offreedom (in addition to the end-e!ector opening and closing), how many scalarquantities need to be measures? What are these scalar quantities and how arethey usually measured? (i.e. what is the name of the primary piece of hardwareassociated with the measurement).


Figure 7.28: (Exercise 7.11) Hybrid position/force controller.

chapter 7 manipulator controlcs336.stanford.edu/slides/cs223a_course_reader_2019... ·...

Documents