Chapter 7 Manipulator Control 7.1 Introduction This chapter - - PDF document

Chapter 7 Manipulator Control

7.1 Introduction

This chapter starts with a review of the basics of PID controllers. That is followed by 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 basics

• f the task-oriented operational space control, which provides dynamic decoupling

and direct control of end-effector motions. Consider the task of controlling the motion of an n-DOF manipulator for some goal configuration defined by a set of desired joint positions. This task can be accomplished by selecting n independent proportional-derivative, PD, controllers that affect each joint to move from its current position to the goal position. Each

• f these controllers can be viewed as a spring-damper system attached to the
• joint. 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 a restoring force that moves the joint back to its goal position. During motion, the damper 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 to perform motion tracking or any task that involves interaction with the environ-

• ment. In motion, the manipulator is subjected to the dynamic forces acting on its
• links. By ignoring these forces, PD controllers are limited in their performance for

these tasks. The control structures needed to address the dynamics of manipulator systems are presented in section 7.6.2. The discussion here focuses on independent PD 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-effector configuration, the manipulator can be directly controlled at the end effector. Imagine placing a 3D spring-damper system at the end-effector itself, instead of the one placed at the

• joints. In that case the end-effector will be attracted to move to its goal position

by the stiffness of the spring, and the stability of the motion will be provided by the damper. A more general way to think about this approach is to imagine the application

• f a force at the end-effector, as the gradient of some attractive potential field,

whose minimum is at the goal position. In that case thers is a need to add some damping 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 the end-effector, which must be produced by the actuators at the joints. This can be accomplished with the transpose of the Jacobian, which relates forces at the end-effector to corresponding torques at the joints. This allows direct control of the effector without requiring any inverse kinematics. The difficulty in practice with such controllers is their limited performance for mo- tion control, since the dynamics of the manipulator are ignored. The incorporation

• f the dynamics for the end-effector control is discussed in section 7.7.1.

Control with Inverse Kinematics

Typically robot control has the following structure Let xd be the desired position and orientation of the end effector. The inverse kinematics 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 desired joint position and actual joint position. These controllers are simple PD or PID

• controllers. Because of the computational complexity of inverse kinematics, this

approach is difficult to use for tasks involving real-time modifications of the end- effector 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 Jacobian and 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-effector 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 effector, this relationship can be used to continuously increment the joint position in accordance with small displacements along the end-effector path. At a given configuration q,

198 CHAPTER 7. MANIPULATOR CONTROL Figure 7.3: Inverse kinematics robot control.

the end-effector position and orientation is determined by the forward kinematics, x = f(q). Selecting a neighboring desired end-effector configuration xd results in a small end-effector displacement. δx = xd − x The corresponding joint displacement δq can be established using the inverse of the Jacobian matrix. δq = J−1(q)δx From the current joint configuration q, this allows to compute the desired joint configuration qd as, qd = q + δq In general, the task transformation problem involves, in addition, transformations

• f the end-effector desired velocities and accelerations into joint descriptions. Task

transformations are computationally demanding and are difficult to carry out in real-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 system shown in Figure 7.5. The study of this natural 1 DOF system will provide the basis for 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 stiffness k. The position of the mass is determined by the coordinate x, and the neutral position of the spring is assumed at x = 0. The kinetic energy of this system is K = 1 2m ˙ x2

200 CHAPTER 7. MANIPULATOR CONTROL

The potential energy of this system is due to the spring. The mass is subjected to the 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 force due to a potential energy. On the right hand side of Lagrange’s equation, the external force is zero. The total energy of the system is therefore constant. Thus this system is stable, but oscillatory. The equation of motion is m¨ x + 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 the potential energy will be transfered into kinetic energy. At the neutral position, the potential energy becomes zero and the kinetic energy starts to transfer back into potential 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 both k and m – higher frequency with higher stiffness 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 + ω2

nx = 0

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

7.2. PASSIVE NATURAL SYSTEMS 201 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 mechanical

• system. Assume that the friction acting on the mass-spring is simply viscous,

ffriction = −b ˙ x With the friction, the Lagrange equation is d dt(∂L ∂ ˙ x ) − ∂L ∂x = ffriction (7.8)

202 CHAPTER 7. MANIPULATOR CONTROL

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

• rder equation

m¨ x + b ˙ x + kx = 0 (7.9) To analyze the characteristics of this system, divide the equation by m.

• k/m

represents the natural frequency of the system and b/m represents the damping of the system. ¨ x + b m ˙ x + k mx = 0 (7.10) The friction results in an oscillatory-damped behavior. As the friction coefficient b increases, the magnitude of oscillations decreases at a faster rate. If b was very large, the system will be over damped. It will never cross the zero axis, slowly moving 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 desirable in the development of control systems. The analysis of the time-response of the system shows that the critically-damped state is reached when b m = 2ωn (7.11)

Figure 7.7: Dissipative Systems.

7.2. PASSIVE NATURAL SYSTEMS 203

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. The natural 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, and

• scillatory when ξn < 1. With ωn and ξn, the equation of motion is

¨ x + 2ξnωn ˙ x + ω2

nx = 0

(7.13) The time-response of this system is x(t) = ce−ξnωnt cos(ωn

• 1 − ξ2

nt + φ)

(7.14)

Figure 7.8: Dissipative Systems Response.

The exponential decreases with the damping ratio, and the frequency of oscillation is affected by ωn

• 1 − ξ2
• n. This quantity is defined as the damped natural frequency,

ω = ωn

• 1 − ξ2

n

204 CHAPTER 7. MANIPULATOR CONTROL

When ξn = 1, the system is critically damped and there are no oscillations. The smaller ξn, the closer is the damped natural frequency is to the natural frequency. The period of the oscillations is

2Π ωn√ 1−ξ2

n .

Example

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

• 1 − ξ2

n then ωn =

• k

m = 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 the robot controller. Consider a robot with 1-DOF prismatic joint, with mass m. The task is to move the robot from its current configuration, x0, to a desired position

• xd. The actuation of this robot involves one force, f, acting on the prismatic link.

The robot equation of motion is m¨ x = f

Figure 7.9: Simple goal control.

The goal is to find the force f that accomplishes the task, while providing passive

• behavior. To achieve this behavior, select f to be conservative – the gradient of a

positive potential function. For this task, the potential function will be designed to 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 m¨ x = − ∂ ∂x[1 2kp(x − xd)2] (7.15) The behavior of the controlled closed-loop system is m¨ x + kp(x − xd) = 0 (7.16) This behavior is identical to that already seen for the conservative mass-spring

• system. In this controlled system, however, the natural springiness, is reproduced

by the parameter kp, which controls the artificial stiffness of the closed loop system. Under this control, the link will oscillate around the desired position xd, in similar fashion to the mass-spring system. The frequency of this oscillation is determined by

• 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

206 CHAPTER 7. MANIPULATOR CONTROL

dissipative force. This non-conservative force will appear on the right-hand side of Lagrange 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 acting to oppose the velocity. Choosing the dissipative force as fd = −kv ˙ x, the asymptotic stability condition for this system becomes ˙ xT (−kv ˙ x) = −kv ˙ x2 < 0, for ˙ x = 0 (7.19) This condition is satisfied if kv > 0. The total control force becomes f = −kp(x − xd) − kv ˙ x (7.20) where kp > 0 and kv > 0. This is simply the conventional proportional-derivative

• control. kp is position gain and kv is the velocity gain. The closed-loop system

corresponding to this control is described by m¨ x + kv ˙ x + kpx = kpxd (7.21) To determine the characteristics of this 2nd order system, divide the above equation by 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 = kp m (7.23) ξ = kv 2

• kpm

(7.24)

7.3. PASSIVE-BEHAVIOR CONTROL 207

where ω is the closed-loop frequency and ξ is the closed-loop damping ratio. Since these two parameters determine the response of the controlled system, first set ω and ξ to achieve the desired response. The position gain kp and the velocity gain kv 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 proportional to the mass m, given a selection of ω and ξ. For a system with unit mass, these gains 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 desired

dynamic response for a unit-mass system. 1.¨ x = f′ with f′ = −k′

v ˙

x − k′

p(x − xd)

and the closed-loop behavior for the single unit-mass system is 1.¨ x + k′

v ˙

x + k′

px = k′ pxd

Given the control gains for a desired behavior of a unit-mass system, the gains that provide the same behavior for an m-mass system are given by kp = mk′

p

kv = mk′

v

and the control f of the m-mass system is m¨ x = f with f = mf′ These relationships play an important role in extending the analysis to systems with nonlinearities and larger numbers of degrees of freedom.

208 CHAPTER 7. MANIPULATOR CONTROL

7.3.1 Nonlinear Systems

Consider again the 1-DOF prismatic arm. A more realistic model of this system will include some amount of friction, which will be approximated by a nonlinear function of the position and velocity, i.e. b(x, ˙ x). The system is now described by the equation m¨ x + b(x, ˙ x) = f (7.29) If it was possible to model the friction b, this model could be used in the control of the system to compensate for this friction, and to control the resulting linearized system 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 nonlinear forces acting on the system, and α is the mass of the system allowing the use of unit-mass system’s control design f′. Since both the mass and the nonlinearities in the 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. m¨ x + b(x, ˙ x) = mf′ + b(x, ˙ x) (7.33) With perfect estimates, ( m = m, and b = b), the closed-loop behavior of this system 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 with k′

p and k′ v to achieve the desired behavior.

This control structure is shown in Figure 7.11. The dotted block in this Figure represents the unit-mass system being controlled by f′ with outputs x and ˙ x.

7.3. PASSIVE-BEHAVIOR CONTROL 209 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 m¨ x + b(x, ˙ x) = f is controlled by selecting f = mf′ + b(x, ˙ x) with f′ = −k′

v ˙

x − k′

p(x − xd)

where k′

p and k′ v are the PD control gains. With perfect estimates, the closed-loop

behavior is described by 1.¨ x + k′

v ˙

x + k′

p(x − xd) = 0

(7.34) A robot task may involve the tracking of a desired trajectory xd(t). In addition to the time-varying desired position, a trajectory tracking task generally involves the desired velocities and accelerations, i.e. ˙ xd(t) and ¨ xd(t). The robot control for this task will have an identical structure to the controller described above, with a new unit-mass control input, f′, designed for trajectory tracking

210 CHAPTER 7. MANIPULATOR CONTROL

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 to position and velocity feedback, trajectory tracking control involves an acceleration feed-forward component that allows the system to better anticipate changes on the desired trajectory.

7.4 Disturbance Rejection

The linear closed-loop behavior we have achieved relies on the assumption of exact estimates of the system’s parameters. The errors in these estimates, in addition to

• ther un-modeled aspects of the system, all contribute to disturbance forces that

could affect the performance and stability of the system. The effects of these dis- turbances can be minimized by appropriate selection of gains, k′

p and k′ v, involved

in 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 the gains, as will be seen in section 7.4.1. Let us assume that all disturbances acting on the system can be represented by a single disturbance force fdist, that is directly acting at the input of system, as illustrated in Figure 7.13. In addition, this disturbance force will be assumed to be constant. The system’s equation of motion becomes m¨ x + 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 = fdist

m (7.39) For a desired position task,

212 CHAPTER 7. MANIPULATOR CONTROL

f′ = −k′

v ˙

x − k′

p(x − xd)

The closed-loop behavior of the system is 1.¨ x + k′

v ˙

x + k′

p(x − xd) = fdist

m

Steady-State Error

The steady-state error is determined by analysis of the closed-loop system at rest, i.e. when all derivatives are set to zero. This leads to the steady-state equation k′

pe = fdist

m (7.40) Then the steady-state error is e = fdist mk′

p

= fdist kp (7.41) Thus high values of k′

p reduce the steady-state error. This also shows that heavy

systems (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 behavior

• f this system is

7.4. DISTURBANCE REJECTION 213

m¨ x + kv ˙ x + kp(x − xd) = 0 (7.42) Let us consider the application of a disturbance force, fdist, to this system, and let us 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 + fdist kp

7.4.1 Control Gain Limitations

The higher kp is, the better the disturbance rejection becomes. However, control gains are limited by various factors involving structural flexibilities in the mecha- nism, time-delays in actuators and sensing, and sampling rates. An increase of kp results in an increase of the closed-loop frequency ω. As this frequency approaches the the first unmodeled resonant frequency, ωlow−resonant, the corresponding mode can 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 time delay, ω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 to further reduce disturbance errors. For a trajectory tracking task, A PID controller (proportional-integral-derivative) involves, in addition to k′

p, and k′ v, the integral

gain k′

• i. A PID controller for a trajectory tracking task is

214 CHAPTER 7. MANIPULATOR CONTROL

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

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

···

e +k′

v¨

e + k′

p ˙

e + 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 effective inertia optimiza-

• tion. Figure 7.15 depicts a gear reduction system.

Figure 7.15: Gear reduction.

The gear ratio is η = R

r , 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 effective inertia, and (bL +η2bm) represents the effective damping both perceived at the link. Finally, the position and velocity gains can be 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 of PD controllers designed independently for each link. While sufficient for pick-and- place tasks, this type of control is limited in its performance for tasks involving

216 CHAPTER 7. MANIPULATOR CONTROL 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 manipulator shown Figure 7.16. The dynamic equation of motion for this manipulator is m11 m12 m21 m22 ¨ θ1 ¨ θ2

• +

m112 ˙ θ1 ˙ θ2

• +
• m122

− m112

2

˙ θ2

1

˙ θ2

2

• +

g1 g2

• =

τ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 ˙ θ2

2 + G1

= τ1 (7.54) m22¨ θ2 + m21¨ θ1 − m112 2 ˙ θ2

1 + 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 decoupled systems described by m11¨ θ1 = τ1 (7.56) m22¨ θ2 = τ2 (7.57) These equations neglect the dynamic forces acting on the joints, and ignore the configuration dependency of the link inertias. The actual system is nonlinear and

7.6. PD CONTROL FOR MULTI-LINK SYSTEMS 217 Figure 7.17: Controller for Two-DOF example.

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

7.6.1 Stability of PD Control

The dynamics of an n DOF manipulator are described by M ¨ q + B(q)[ ˙ q ˙ q] + C(q)[ ˙ q2] + g(q) = τ (7.58) With PD control, the joint torques are selected as τ = −kp(q − qd) − kv ˙ q (7.59)

218 CHAPTER 7. MANIPULATOR CONTROL Figure 7.18: Coupled n-DOF control.

The stability of this system can be easily concluded, as all external forces acting

• n this system are either conservative (−kp(q − qd), gradient of a potential) or

dissipative (−kv ˙ q). That is τ = −∇qVd − kv ˙ q (7.60) where (Vd = 1 2kp(q − qd)T (q − qd)

7.6. PD CONTROL FOR MULTI-LINK SYSTEMS 219

To further analyze the stability, consider again Lagrange’s equation, from which equation 7.58 was obtained. d dt(∂K ∂ ˙ q ) − ∂(K − Vgravity) ∂q = −∇Vd − kv ˙ q (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 = −kv ˙ q 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, under the condition ˙ qT τdissipative < 0 which is verified for the dissipative force −kv ˙ q if kv > 0. A manipulator controlled with a set of independent PD controllers is stable, as the effect 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 ignores the dynamic coupling forces. High gains provide better disturbance rejection, but as mentioned earlier, control gains are limited by the system’s flexibilities, time delays and sampling rate. Dynamic decoupling and motion control of the robot system can be accomplished by a control structure that uses the manipulator dynamic model. The manipulator dynamics are described by M(q)¨ q + v(q, ˙ q) + g(q) = τ (7.63)

220 CHAPTER 7. MANIPULATOR CONTROL

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-loop behavior 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′

v ˙

e + 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-mass systems, controlled by τ ′. The control input to the decoupled system was selected as 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-effector
• descriptions. In which case, the end-effector task must be first transformed into

joint 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-effector mo- tions, eliminating the need for task transformation. The problem of task trans- formation and joint coordination is yet more difficult for manipulation involving redundant mechanisms or multiple robots. The manipulation of an object, for example, by two robots, as illustrated in Figure 7.20, requires complex real-time

• coordination. This becomes unnecessary with the direct control of the manipulated
• bject 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 effector by

222 CHAPTER 7. MANIPULATOR CONTROL

a potential function whose minimum is at the end-effector 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 gradient

• f this potential,

F = −∇xVgoal(x) This force will be produced by a torque vector at the joint of the robot, given simply by τ = JT F Other more complex behaviors can be simply created by the design of artificial potential 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 damping forces, for instance Fd = −kv ˙ x

7.7. OPERATIONAL SPACE CONTROL 223

The corresponding torques are τd = JT (−kv ˙ x) 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 (JT J) ˙ q)] < 0 For a non-redundant manipulator and outside of singularities, JT J, is a positive definite matrix, and the quantity [ ˙ qT (JT J) ˙ q)] is positive. The system is then asymptotically stable if kv > 0.

7.7.1 Operational Space Dynamics

The description of the dynamics at the end-effector requires first to select a set of generalized coordinates, x, that represent the end-effector position and orientation, e.g. (x, y, z, α, β, γ). The kinetic energy of the system can then be expressed as a quadratic form of the generalized velocities, ˙ x, Kx = 1 2 ˙ xT Mx ˙ x (7.75) where Mx represents the mass matrix associated with the inertial properties at the end-effector. Let F be the vector of generalized forces corresponding to the generalized coordinates x. The end-effector equations of motion are d dt(∂K ∂ ˙ x ) − ∂(K − V ) ∂x = F (7.76) which can be developed in the form

224 CHAPTER 7. MANIPULATOR CONTROL

Mx¨ x + 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 of the system is Kq = 1 2 ˙ qT M ˙ q (7.77) where M is the joint space mass matrix. Expressing the fact that Kx = Kq establishes the relationship JT MxJ = M (7.78) which leads to Mx = J−T MJ−1 (7.79) The relationship between the gravity force vectors g and gx is simply given by the transpose of the Jacobian matrix. g = JT gx The relationship between v and vx involves the time derivatives of the Jacobian matrix (¨ x = J¨ q + ˙ J ˙ q). In summary these relationships are Mx = J−T MJ−1 (7.80) vx = J−T v − Mx ˙ J ˙ q (7.81) gx = J−T g (7.82)

7.7.2 Operational Space Dynamic Control

The end-effector dynamics is described by the equation Mx¨ x + 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-effector 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-effector motion, xd, ˙ xd and ¨ xd.

7.8 Exercises

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

226 CHAPTER 7. MANIPULATOR CONTROL

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

• 0. If

q =

• 0.2618

0.5236 T radians, ˙ q = 0, and ¨ q =

• 0.0

10.0 T rad sec2 , 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 rad sec , ¨ q =

• 0. At some

point during the motion, the configuration is the same as in part (a). What is the disturbance torque at each joint due to centrifugal/coriolis forces at that instant? (c) Finally, return the manipulator to the vertical plane so that gravity forces are present. We will now apply motor torques which correspond to the PD controller: τ = − 100 100

• (q − qd) −

10 10

• ˙

q Once the manipulator has come to rest using this control law with some desired position, the actual position reading obtained from the joint posi- tion sensors is q =

• 0.2618

0.5236 T radians. 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 Figure 7.22 is given as, τ1 τ2

• =

4 + 2C2 1 + C2 1 + C2 1 ¨ θ1 ¨ θ2

• +

−S2( ˙ θ2

2 + 2 ˙

θ1 ˙ θ2) S2 ˙ θ2

1

• +

(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-linear model-based portion with goal position (¨ qd = ˙ qd = 0) to critically damp the system at all times. τ = ατ ′ + β τ ′ = − K′

V1

K′

V2

• ˙

q − K′

P1

K′

P2

• (q − qd)

(a) Find α and β.

7.8. EXERCISES 227

X0 Y θ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-state

error, e2 is less than or equal to 0.5% of the disturbance force, τdist2 in any

• configuration. 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 that its XoYo plane is on a frictionless table and its Zo axis is pointing upward from the table. In other words, all links of the manipulator are lying on the table with their joint axes are vertical to the table surface. Since there is no friction, the manipulator is now floating on the table and the gravity is now acting on the negative Zo direction. Notice that the gravity has no effect on the manipulator in this setup since all links are supported by the frictionless table, i.e., G(q) = 0. Once the manipulator has come to rest using this control law with some desired position, the actual position reading obtained from 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 if g = 9.81m/s2? (f) What was the desired position of the manipulator from part (e)?

228 CHAPTER 7. MANIPULATOR CONTROL

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 the system 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 the resultant steady-state position error of the controlled system? Assume position error 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: Its equations of motion were derived in the text and are shown here: τ1 = (m1L2

1 + Izz1 + Izz2 + m2d2 2) ¨

θ1 + 2m2d2 ˙ θ1 ˙ d2 + (m1L1 + m2d2)g cos(θ1) τ2 = m2 ¨ d2 − m2d2 ˙ θ1

2 + 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 is between 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, decouples the dynamics, and tracks a desired trajectory (ie. position, velocity and acceleration) 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 joint velocities ( ˙ θ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 natural frequency 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), when d2 = 0.6m. For joint 1, what is the effective inertia “seen” by the joint if we have 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.

• f mass of link 2 to the end-effector is L3. In this case, the end-effector position

in the plane is:

230 CHAPTER 7. MANIPULATOR CONTROL

0Pend =

• s1(d2 + L3)

−c1(d2 + L3)

• (d) Use 0Pend to compute the Jacobian for linear velocity at the end-effector in

frame {0}. (e) Using your answer from part (d), the configuration θ1 = 45◦, d2 = 0.6m, and assuming 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, 4¨ x + 20 ˙ x + 25x = f. (a) Find the natural frequency ωn and the natural damping ratio ζn of the natural (passive) system (f = 0). What type of system is this (oscillatory,

• verdamped, etc.) ?

(b) Design a PD controller that achieves critical damping with a closed-loop stiffness kCL = 36. In other words, let f = −kv ˙ x − kpx, and determine the gains kv and kp. Assume that the desired position is xd = 0. (c) Assume that the friction model changes from linear (20 ˙ x) 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 all times and maintain a closed-loop stiffness of kCL = 36. In other words, let f = αf′ + β and f′ = ¨ xd − k′

v( ˙

x − ˙ xd) − k′

p(x − xd). Then, find f,α,β,f′,k′ p

and 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 given by 4 + c2 1 + c2 1 + c2 1 ¨ θ1 ¨ θ2

• +

−s2( ˙ θ2

2 + 2 ˙

θ1 ˙ θ2) s2 ˙ θ2

1

• =

τ1 τ2

• (a) Assume that joint 2 is locked at some value θ2 using brakes and joint 1 is

controlled with a PD controller, τ1 = −40 ˙ θ1 − 400(θ1 − θ1d). What is the minimum and maximum inertia perceived at joint 1 as we vary θ2? What are the corresponding closed-loop frequencies?

7.8. EXERCISES 231

(b) Still assuming that joint 2 is locked, at what values of θ2 do the minimum and 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, velocity and acceleration) which will provide a closed-loop frequency of 10 rad/sec

• n joint 1 and 20 rad/sec on joint 2 and be critically damped over the entire
• workspace. That is, let

τ ′ = ¨ θd − k′

v1

k′

v2

• (˙

θ − ˙ θd) − k′

p1

k′

p2

• (θ − θd)

then find the matrices α and β and the vector τ, along with the necessary gains k′

vi and k′ pi.

(d) If θ2 = 180◦, what is the steady-state error vector for a given disturbance torque, τ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-damped

• r critically damped?

232 CHAPTER 7. MANIPULATOR CONTROL

Exercise 7.8. Design a trajectory following controller with the structure shown in Fig. 7.26 for a system with dynamics f = Ax2 ˙ x¨ x + B ˙ x2 + C sin x such that errors are suppressed in a critically damped fashion over all configurations and the closed loop stiffness 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 are given by

Figure 7.27: (Exercise 7.9) RR planar manipulator.

7.8. EXERCISES 233

τ1 =m2l2

2( ¨

θ1 + ¨ θ2 + m2l1l2c2(2¨ θ1 + ¨ θ2) + (m1 + m2)l2

1 ¨

θ1 − m2l1l2s2 ˙ θ2

2

− 2m2l1l2s2 ˙ θ1 ˙ θ2 + m2l2gc12 + (m1 + m2)l1gc1, τ2 =m2l2c2¨ θ1 + m2l1l2s2 ˙ θ2

1 + m2l2gc12 + m2l2 2(¨

θ1 + ¨ θ2). Using the symbols in these equations, give explicit equations for computing the values of τ2 for a joint based controller which will. Use a model based control system as given by τ = ατ ′ + β to produce decoupled control with a servo law of the form τ ′ = ¨ θd + Kv ˙ E + KpE with Kp = 100 110

• and

Kv = 29 35

• Give your results for the case where the joint angles and trajectories of each joint

are 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: m1¨ x1 + k1x1 − k1x2 = F1 m2¨ x2 + (k1 + k2)x2 − k1x1 = F2 We desire critically damped response to errors and a closed loop stiffness of KCL = 100 for both degrees of freedom. Exercise 7.11. In order to implement a hybrid controller of the type in diagram 7.28 (including the omitted velocity loop), if the manipulator has 6 degree of freedom (in addition to the end-effector opening and closing), how many scalar quantities need to be measures? What are these scalar quantities and how are they usually measured? (i.e. what is the name of the primary piece of hardware associated with the measurement).

234 CHAPTER 7. MANIPULATOR CONTROL Figure 7.28: (Exercise 7.11) Hybrid position/force controller.