:31... Sun.
1

.;Aa....l‘. A
; . W\1.1:..l
. IA r‘].

. .1.
24.11.}-.1
..

 

Jwvfiwfl

‘ , f .. 1“}:
i V ., .1... 5-. .n , 130.74.
(.2. L.>\. n ‘ . . \Ll. .

. p 3:
,. : .3 5.1 1......
‘ I4 .1»)... 3 A? l. l ‘

 

 

 

 

lW‘li‘i‘lii“itii‘iiiilm

Liana“ , 2
Michigan met-s
University

This is to certify that the
dissertation entitled

COMPLIANT CONTROL OF CONSTRAINED
ROBOT MANIPULATORS

presented by

Choong Sup Yoon

has been accepted towards fulfillment
of the requirements for

Ph.D. degree in Electrical Engineering

 

 

 

Major professor

Date Nnggbr 1;; [789

MS U is an Affirmative Action/Equal Opportunity Institution

PLACE IN RETURN BOX to remove this checkout from your record.
TO AVOID FINES return on or before date due.

 

DATE DUE DATE DUE DATE DUE

ll
_Jl
fl

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

mar—“1!

MSU Is An Affirmative Action/Equal Opportunity Institution _7

 

 

W

COMPLIANT CONTROL OF CONSTRAINED
ROBOT MANIPULATORS

By

Choong Sup Yoon

A DISSERTATION

Submitted to
Michigan State University
in partial fulfillment of the requirements
for the degree of

DOCTOR OF PHILOSOPHY

Department of Electrical Engineering

1989

ABSTRACT

COMPLIANT CONTROL OF CONSTRAINED
ROBOT MANIPULATORS

By

Choong Sup Yoon

A new method for the compliant control of robot manipulators is presented. We
formulate the compliant control problem mathematically employing the framework of
constrained Hamiltonian systems. We then derive nonlinear control expressions for the
force and the motion on the constraint surface. The control strategy consists of the
sum of two nonlinear controls: the force part and the motion pan. The force control
restricts (the end effector of) the manipulator to the constraint surface and the motion
control steers (the end effector of) the manipulator along a specified path on the con-
straint surface. The derivations reveal conditions that define the class of constraint
surfaces allowable in the formulations. Two examples are then given to illustrate the

formulation and the methodology.

We then consider the feedback of error signals with respect to a desired position,
velocity and force. We also allow for uncertainty in modeling and for external distur-
bances. We show that the introduced nonlinear feedback controls Specify the dynamics

of the manipulator onto the constraint surface.

Finally, we present computer simulations which verify the analytical formulation

and the stabilization control.

ACKNOWLEDGMENTS

I would like to thank my major advisor, Dr. Fathi M.A. Salam for his patience,
guidance and encouragement during the course of this research. Thanks to my gui-
dance committee: Dr. Hassan K. Khalil, Dr. Steven W. Shaw and Dr. Charles R.

MacCluer for their comments and suggestions.

Finally, I wish to express my sincere thanks and appreciation to my parents and
my wife parents for their continued prayers and. encouragement. Special thank to my
wife, Kyung Soon, and my children, Sung—Min, Jung-Uk, help and understanding

throughout my studies.

iii

TABLE OF CONTENTS

LIST OF TABLES .....................................................................................................

LIST OF FIGURES ...................................................................................................

Chapter 1:
Chapter 2:

Chapter 3:

3.1
3.2

Chapter 4:

4.1
4.2
Chapter 5:
5.1
5.2

Chapter 6:

Chapter 7:

Chapter 8:

INTRODUCTION ..................................................................................
LITERATURE REVIEW .......................................................................

MODELING MANIPULATORS AS HAMILTONIAN
SYSTEMS ...............................................................................................

Free Hamiltonian Systems on manifolds ...............................................

Hamiltonian Systems with Constraints ..................................................

THE CONTROL OF CONSTRAINED HAMILTONIAN
SYSTEMS ...............................................................................................

The Control of The Contact Force ........................................................
The Control of Compliant Motion .........................................................
THE REQUIRED TORQUE INPUT ....................................................
Special Constraint functions ...................................................................
General Constraint functions ..................................................................

APPLICATION OF THE CONTROL LAWS
TO EXAMPLE MODELS ......................................................................

STABILIZATION ON THE CONSTRAINT SURFACE
FOR THE EXAMPLE MODELS ..........................................................

SIMULATION ........................................................................................

iv

vi

vii

l

5

18

18

21

26

26

31

35

37

38

40

54

61

Chapter 9: SUMMARY, DISCUSSION, AND CONCLUSIONS .......................... 76

LIST OF REFERENCES ........................................................................................... 79

LIST OF TABLES

Table 1. The physical parameters of the 3 d.o.f. manipulator ............................. 61

vi

Figure 2.1.
Figure 2.2.
Figure 2.3.
Figure 2.4.
Figure 2.5.

Figure 3.1.

Figure 3.2.
Figure 4.1.

Figure 4.2.

Figure 4.3.
Figure 4.4.
Figure 6.1.
Figure 6.2.
Figure 6.3.

Figure 8.1.

Figure 8.2.
Figure 8.3.
Figure 8.4.

Figure 8.5.

Figure 8.6.

Figure 8.7.

LIST or FIGURES

Compliant motions .......... ~ ....................................................................... 6
A peg being inserted into a hole ........................................................... 7
A robot manipulator gripping a load .................................................... 13
A robot with its end in contact with a rigid constraint surface ........... 15
Two robots gripping a load ................................................................... 16
The symplectic orthogonal space .......................................................... 23
The decomposition of the Hamiltonian vector X” ................................ 25
The (constraint) surfaces C and E ......................................................... 29
The decomposition of the constrained Hamiltonian vector ................. 30
The (constraint) surfaces C and S .......................................................... 32
The force control a and the motion control a ....................................... 34
A 3—degree of freedom manipulator .................................................... 40
Physical constants of link i ................................................................... 41
The constraint surface of example 2 ..................................................... 48
Plot of the (generalized) angle q1 versus time, Example 2 .................. 63
'Plot of the (generalized) angle q’ versus time, Example 2 .................. 63
Plot of the (generalized) angle q3 versus time, Example 2 .................. 64
Plot of the (generalized) velocity q‘ versus time, Example 2 .............. 64
Plot of the (generalized) velocity 42 versus time, Example 2 .............. 65
Plot of the (generalized) velocity 43 versus time, Example 2 .............. 65
Plot of the force control :2, versus time, Example 2 ............................ 66

vii

Figure 8.8. Plot of the force control .2, versus time, Example 2 ............................
Figure 8.9. Plot of the motion control it, versus time, Example 2 .........................
Figure 8.10. Plot of the motion control :12 versus time, Example 2 .......................
Figure 8.11. Plot of the motion control it; versus time, Example 2 .......................
Figure 8.12. Plot of the motion control it, versus time, Example 2 .......................
Figure 8.13. Plot of the Lagrangian multiplier 1, versus time, Example 2 ............
Figure 8.14. Plot of the Lagrangian multiplier k2 versus time, Example 2 ............

Figure 8.15. Plot of the (generalized) angle q1 versus time,

Example 2 (the overall nonlinear control) ......................................................

Figure 8.16. Plot of the (generalized) angle 42 versus time,

Example 2 (the overall nonlinear control) ......................................................

Figure 8.17. Plot of the (generalized) angle q’ versus time,

Example 2 (the overall nonlinear control) ......................................................

Figure 8.18. Plot of the (generalized) velocity é‘ versus time,

Example 2 (the overall nonlinear control) ......................................................

Figure 8.19. Plot of the (generalized) velocity 4’ versus time,

Example 2 (the overall nonlinear control) ......................................................

Figure 8.20. Plot of the (generalized) velocity 43 versus time,

Example 2 (the overall nonlinear control) ......................................................

Figure 8.21. Plot of the force control 131 versus time,

Example 2 (the overall nonlinear control) ......................................................

Figure 8.22. Plot of the force control :22 versus time,

Example 2 (the overall nonlinear control) ......................................................

Figure 8.23. Plot of the motion control it, versus time,

Example 2 (the overall nonlinear control) ......................................................

viii

66

67

67

68

68

69

69

7O

7O

71

71

72

72

73

73

74

Figure 8.24. Plot of the motion control :72 versus time,

Example 2 (the overall nonlinear control) ...................................................... 74

Figure 8.25. Plot of the motion control :73 versus time,

Example 2 (the overall nonlinear control) ...................................................... 75

Figure 8.26. Plot of the motion control a, versus time,

Example 2 (the overall nonlinear control) ...................................................... 75

ix

CHAPTER 1

INTRODUCTION

Currently most robots are used for very limited tasks which is usually character-
ized by position-to—position movements, e.g., piek-and-place, spot welding and spray
painting. Other essential but complicated tasks involve contact with the manipulator’s
environment, e.g., inserting a pin into a hole, assembling, plasma welding, contour fol-
lowing, debuning, grinding, etc., see [1, 2]. Such contact usually results in the genera-
tion of external ferces acting on the end effector of the manipulator. External contact
forces such as the ones inuoduced by constraint surfaces always modify the dynamical
behavior of a manipulator. Consequently, issues of appropriate modeling and of

effective new control strategies arise.

Compliant control is concerned with the control of a robot manipulator in contact
with its environment, see [3-7]. The end effector of the manipulator first converges to
the constraint surface at a specified position generating a specified force upon contact.
Then, the end effector moves along a desired path on the surface while maintaining a
desired contact force profile (along this path). Thus, compliant motion calls for the
input torque to achieve tracking for a specified path on the constraint surface, and with

a specified contact force.

In principle, such tracking is possible because the movement of the end effector is
limited to a submanifold on the constraint surface and consequently frees some corn-
ponents of the input torque to control the contact force upon the surface. However, the
nonlinearity of the governing dynamics as well as the constraint equations could poten-
tially make the control process difficult if not impossible. The difficulty may translate

mathematically to the presence of singularities at some points on the constraint surface

or to the lack of well-posedness of the governing system of dynamic equations with

algebraic constraints.

We choose to formulate the problem in the joint space. An advantage of this
choice is that the constraint now applies to the joint angles directly; consequently the
constraint applies to the links of the manipulator as oppose to its end effector as it is
the case in the task space formulation. This makes it convenient for the control action
which manipulators the torques to control the angles directly. Another advantage is
that once the class of allowable constraints characterized by our formulation is
identified in the joint space, the simpler and direct use of the forward kinematic
transformation would provide the corresponding class of the allowable constraint sur-
faces in the joint space. We remark, however, that determining useful class of surfaces,
from the view point of applications, is a nontrivial research problem that still needs

investigation.

The control process we envision may take the following steps. The end effector is
first steered to a point on the constraint surface using, e.g., the linear feedback control
strategies reported in [8-11]. In addition, one must also guarantee that at the final
(desired) position on the surface, a specified force (normal to the constraint surface) is
generated. Once the end effector is located at a specific position and with a specified
force, one may then apply compliant control strategies to generate or to track a desired
path with a desired contact force profile. Some results on compliant control have been
reported in [3-7].

In this work, we propose a control strategy which consists of the sum of two
nonlinear controls. One control restricts (the end effector of) the manipulator to the
constraint surface; this control represents the force control part. The other control
steers (the end effector of) the manipulator along a specified path on the constraint sur—
face; this control represents the position control part. Then we show that these non-

linear controls can be supplied by the input torque vector at the joints. Specifically,

we give an expression for the (physical) torque which would generate the desired non-
linear controls. (It is possible to include the dynamics of the actuators and consider the

actuator voltages as the physical inputs.)

We employ the geometric tools of symplectic Hamiltonian systems in setting up
our framework. Although these tools have been used in [3], our emphasis is quite
different: we assume that the amplitude (modulo a multiplicative constant) of the
desired force is given as a function defined on the constraint surface; then we derive
the control required to maintain that desired force. We also derive the second com-
ponent of the control strategy which generates desired paths or trajectories on the con-
straint surface. The derivations require that the constraint surfaces satisfy conditions in
terms of a matrix of Poisson brackets being nonsingular. These conditions in fact
specify the class of constraint surfaces allowable in our formulation. We remark that
the mathematical formulation and the main computational tool, namely, the Poisson
bracket, are not adopted here because of personal inclination and preference for com-
plexity. They are adopted here because of necessity. They represent the only accurate,
precise way of decomposing the vector field of the dynamics of the nonlinear system
(the manipulator) at every point onto the constraint surface into two components: an
orthogonal component and a tangential component to the constraint surface. In addi-
tion, it should be recognized that the theoretical framework provides guidance and

deep insights into how to properly devise and apply the control strategies.

As a next step, we apply, in addition, a linear controller to the resulting compliant
system. This linear controller takes advantage of feeding back error signals, with
respect to a desired position, velocity and force. For given constraint equations, the

additional linear controller may depend on the variables of all joints.
The organization of this dissertation is as follows. In chapter 2, previous works

on the compliant control of robot manipulators are briefly presented. The methodolo-

gies in these works classify the compliant control into passive mechanical compliance

and into active compliance. It is observed that these previous works have not
rigorously taken into account the dynamic equations governing the constrained robot
manipulator, nor have they substantiated analytically that the desired position and
desired force trajectories can be simultaneously achieved. Furthermore, we describe
examples of mathematical modeling of constrained robot systems [12] which are
described via singular differential equations, i.e., differential equations and algebraic
equations. Sec {12-171, for example. We then describe the principal problem of com-
pliant control. In chapter 3, we are concerned with the modeling of a manipulator
employing a Hamiltonian structure, both free of contact and in contact with a con-
straint surface. In Chapter 4, the main results are derived: an expression for a (non-
linear) force control on the contact surface; then, a control expression for compliant
motion (on the contact surface). In chapter 5, we compute the required physical torque
which would supply the two derived control expressions. In chapter 6, we present two
examples to illustrate the methodology. In Chapter 7, an additional linear (feedback)
controller is introduced to the resulting compliant model for our two examples. The
linear feedback controller achieves the stabilization of the overall compliance-
controlled robot system. In Chapter 8, computer simulations are presented which verify
the effectiveness of the overall nonlinear control strategies introduced. Finally, chapter

9 provides summary, discussion and conclusions of this dissertation.

CHAPTER 2

LITERATURE REVIEW

Compliant motion means that the position and velocity of a robot manipulator are
constrained by the task: the robot interacts with its environment. Examples of compli-
ant motion are contour following, grinding, cutting, drilling, inserting, fastening,
assembly related tasks, etc.. Figure 2.1 shows some examples of compliant motions.

There are two primary methods for producing compliant motion: a passive
mechanical compliance built into the manipulator, and an active compliance imple-
mented into the (software) control loop, often called force control. Passive compliance,
if successful, offers some performance advantages, but the force control methods offer

the advantage of programmability.

Passive compliance

Figure 2.2 shows a typical example of a passive mechanical compliance using the
center compliance linkage [18, 19] mounted to the end-effector of a robot manipulator.
This center compliance linkage allows the robot to give a little rotation when it

encounters the object to be inserted.

For example, there may be a jam if the peg is not aligned in the direction of 2
when the peg is to be inserted into the hole. This effect of misalignment of a piece
part and the robot may result in failure of the task process and in damage to the robot
manipulator and the piece. The only way to free it is to rotate the peg towards the axis

if the hole while the peg is in contact with the hole. The center compliance linkage

provides a sideway movement compensated for by the linkage such that the peg rotates

about the x direction.

I
N /.

 

 

 

 

 

 

 

 

 

 

contour following

inserting a peg in a hole

 

 

 

 

 

 

 

o enin a door
P g moving an object on a belt

Figure 2.1. Compliant motions

Center Compliance Linkage

 

 

 

 

 

 

 

 

 

 

 

 

Peg
Hole

 

 

 

 

Figure 2.2. A peg being inserted into a hole

Force control

In the force control the problem arises as how to relate the requirements of a task
to the motions required and forces anticipated so that force-motion strategies could be
formulated. Various force control techniques have been proposed, but development of
an underlying theory of these techniques has not materialized. Most of these tech-
niques are motivated by concepts and ideas from linear systems and they ignore the
(nonlinear) dynamics of the manipulator and its interaction with the constraint. In this

section, the main efforts in the development of force control are briefly delineated.

In what follows the torque at the joints 1 related to the expression of the force f
vie the equation: 1: = -JT(q)f, where I (q) is the Jacobian of the kinematic function rela-

tion joint angles to task q coordinates.

1) The generalized spring method

This method feeds back force information through a stiffness matrix to a position

control for generating force and can be modeled by a matrix version of Hook’s law as

[20-22],
f = K07 ’Po)
where f is a vector of forces and torques,

p is a vector of position and orientation of the manipulator; (x,y,z,6,,0,,0,)T,

p0 is a vector of desired position and orientation, which is supplied by the

planning system or the user program,

K is the stiffness matrix, which relates forces observed at the effector to devi-
ations from the desired position. This stiffness matrix can be chosen to optim-

ize the performance of a particular task.

In the case of inserting a peg in a hole, the stiffness matrix K can be selected in
such a way that the peg is complying with forces along the x— and y—axes and with
torques about x— and y—axes, and follows a trajectory straight down the middle of the

hole. The K matrix for this task have been postulated to be in the form:

0000-
0000
k,000
0k,00’
00k,0
000k,

OOOOJ’O

 

 

'ooooogrj

with k, a small number and k, a large number. The motions corresponding to k,
(z, 0,) are position-controlled, while for k, are force-controlled. Note that the center

compliant device can be characterized as a passive generalized spring.

2) The generalized damper method

This generalized damper method is similar in form, to the generalized spring method
but it assumes a velocity controller instead of a position controller. This method can be

modeled by the relation [23-25],
f = 80’ - V0),

where v is a vector of velocity and angular velocity of the manipulator,
(itytisépéyaéz)Ts
Va is a vector of desired velocity and angular velocity, which is input from the
planning system or user program,
B is the damping matrix which have negative damping coefficient.

In early examples, no contact force will be maintained when assembly is com-
pleted except in the insert direction velocity (i). In other words, the feedback gain
should be large in the direction (2', 8,) where the task is expected to yield and low

(£93,819) where the task is expected to resist deflection; this is the same as before.

lO

3) The impedance control

It is a generalization of the previous two methods as in [26]. Now

f=K(P ”170) +30) — V0).

4) The explicit force control

Unlike the previous methods, this one has a desired force input rather than posi-

tion or velocity input [27];
5f = F—Fd,

where 5f is the deviation in forces and torques of the manipulators from prescribed

forces and torques,
F is a vector of actual forces and torques, and

F d is a vector of desired forces and torques.

5) The hybrid force-position control

A hybrid controller commands force along certain degrees of freedom, and posi-
tion along the remaining degrees of freedom. The task degrees of freedom is the
number of independent coordinates required to specify completely the position and
orientaion of a system: translation along each of the three axes, and rotation about
each of three axes. For given tasks, these degrees of freedom are specified in the form
of compliance, allowing the user to define which are position-controlled and which are

force-controlled.

As in the case of a peg in a hole, translation of, and rotation about, the z—axis are

position controlled, while the other degrees of freedom are force controlled.

11

5.1) Discrete method [28,29]

The discrete method works as follows. Each programming instruction is written in
primitives defined at a lower level. The lowest level of a hierarchical structure pro—
vides the interface with the sensors and actuators of a robot manipulator. The user con-
structs strings of programming instruction for given tasks, then each instruction exe-
cutes control strategy by combining input from higher level code and from sensors to
provide signals for the actuators.

For the case of inserting a peg into a hole, the programming instructions are
given by

MOVE TO D WITH
[ FORCE X=O
FORCE Y=O
TORQUE X=0
TORQUE Yw ],
where D is the goal position of the peg, i.e., move to D until no force is detected and
rotate until no torque is detected. As shown in the programming instructions, the com-

pliance axes are the translation and rotation of x and y in the task coordinate system.

Therefore, the non-compliance axes are the translation and rotation of z.

5.2) Continuous method

The basic idea of this method is based on coordinated continuous axis motion by
selection of a vector which would determine which world or hand axes are to be force

controlled and which are to be position controlled.

12

For example, a hybrid controller is proposed in [5] as

N

T: = ZIFUBI'M] + VinI-Sfimfll
j=l

where t,- : torque applied by the i"I actuator,
Afl- : force error in the 1‘" degree of freedom,
Ax,- : position error in the 1‘" degree of freedom,
I},- : force compensation function (PI),
‘l’ij : position compensation function (PID),

s. ° component of compliance selection vector that selects which world or

J .
hand axes is to be force controlled or which is to be position controlled.

This method deals with the situation where the position of the end-effector must
be controlled in certain directions and the force must be controlled in the remaining
other directions by the selection vector. Note that the constraint equations and their
complement orthogonal equations in the task space do not transform into orthogonal
pair in the joint space. Consequently, this method can not in actuality decouple the

control of force and position.

We remark that this method is conceptually much clearer than the previously
described ones; however, it still lacks an analytically sound base due to the unavoid-

able interaction between the force and position components in the torque elements.

For the application of robot manipulators to complex tasks which can not be
defined solely in terms of the motion of the end-effector, it is necessary to properly
formulate the compliance problem which would include the dynamic equations of the

robot manipulators as well as their constraint equations.

13

Singular Differential Equation in Constrained Robot Systems

Mathematical models of a robot system which is in contact with its environment
naturally give rise to a mathematical system of differential equations with algebraic
equations. The latter can be viewed as a singular system of differential equations. Fol-
lowing [12], we show examples of modeling singular differential equations of robot
system configurations
I) A Robot Manipulator Gripping a Load

The end-effector of a robot manipulator grips a load as shown in Figure 2.3.

Figure 2.3. A Robot Manipulator Gripping a Load

14

The vector form of the equation of motion of the system shown in Figure 2.3 are
n+m differential equations with m algebraic equations in the n+2m unknowns q, x and
f. It is given by

if
M(q) o o q T - G(q.ri) + 1th

0 ml 0 if: -f-mg ,
0 0 0 fj M(q)—x

 

 

where xe R’" is the position vector of the load in the fixed workspace coordinate sys-

tem,

qe R" is the vector robot joint angles (q, q' denote the velocity and acceleration

of q, respectively),

fe R’" is the contact force, in the task coordinates, between the robot and the

load,
M(q) is the inertial matrix of a robot manipulator,

G(q,q) is a vector function which characterizes the Coriolis, centrifugal and

gravitational force of the robot,

T is the torque vector at the joints,

J(q) = 31;; ) is the Jacobian matrix, here K (q) is a vector function represent-

ing the direct kinematic relation of the robot: 1: = K(q).

15

II) A Robot with its End in Contact with a Rigid Constraint Surface

Consider the end effector of robot contact with a rigid constraint surface as

shown in Figure 2.4.

 

 

Figure 2.4. A Robot with its End in Contact with a Rigid Constraint Surface

The dynamic equation of Figure 2.4 can be represented in vector form as,

M(q) o 2; _[— Gm) + T + farm»).
0 0 7. “ ¢<K<q>>

23:32 is a gradient of the

constraint function (<D(x) = O), and A. is a scalar multiplier for the constraint

where f = Q7002. is the contact force vector; here Q(x) =

function.

Here, we have n differential equations and 1 algebraic equation in the n+1 unknowns

qand A.

16

111) Two Robots Gripping a Load

Consider the case that two robot manipulators grip a load as shown in Figure 2.5.

 

 

 

 

 

 

Figure 2.5. Two Robots Gripping a Load

The dynamic equation is given in vector form as

r - r- a

9141(q1) O 0 O 0. 51 Tr‘Gr(41fl.r)+-’i‘(qufr
0 M2(42) 0 0 0 31.2 Tz ' 02(42Ji2) + 15(47113

 

 

0 0 "1,00 i: -f1-f2-mg ,
O O O O 0 V1 M1(41)-X
. 0 0 0 0 0.

 

 

 

 

{'2‘ i M2(47) " x

where the dimensions of x, ql, and q2 are m, n1, and n2, respectively.

Here, we have n1+n2+m differential equations and 2m algebraic equations in the

n1+n2+3m unknowns q1, q2, 1:, f1 and f2.

To apply control to the constrained system where its dynamics occur within a

(2n-m)-dimensional state space due to m-dimensional constraint equations, one may

17

carry out a reduction of the dimension of the overall system. The reduction amounts to
solving the constraint equations, i.e., obtaining expressions of m state variables in
terms of the remaining variables. If such a reduction can be carried out, then reduced
dynamic equations are obtained. In [4], for instance, a nonlinear canonical transforma-
tion based on the local implicit function theorem. is presented. By using the implicit
function theorem, the reduced dynamic system may become nonphysical at points
where the nonlinear canonical transformation becomes singular. At such impasse points
the reduced system is not well posed. In the following, we pursue analysis using the
Hamiltonian framework for constrained dynamic systems which is valid globally. We

are then able to formulate the compliance control problem in a globally sense.

CHAPTER 3

MODELING MANIPULATORS AS HAMILTONIAN SYSTEMS

3.1 Free Hamiltonian Systems on manifolds

Let M be a 2n-dimensional differential manifold and let w2 be a symplectic struc-
ture on M. Denote the symplectic manifold as the pair (M,w2). See [30-33] for more

discussion of symplectic manifolds.

For finite dimensional manifolds, w2 is given in local coordinates by

w2 = dq‘Adp‘ i: 1,..., n, (31)

where the notation A represents the wedge (or the exterior) product.

Let H : M —) R be a scalar function defined on the symplectic manifold. Then
dH : M —9 T*M is a differential l-form on M, where (111 is the (locally) differential of
H and 7"‘M is the cotangent bundle of M. The function H is called Hamiltonian func-
tion. These geometric notions will be needed for the development later.

Now let the local canonical coordinates on the 2n-dimensional symplectic mani-
fold be (ql, ..., q”; pl, ..., p") := (q ; p). Denote the Hamiltonian vector field associated
with the Hamiltonian H by X”. The equation that associates a unique vector field X” to

Hbywzis

the? = dH, (3.2)

where ixuw2 is interior product (the contraction of X and w of 2-form w2 with a vector

field X). Let the tangential vector Xe TM, where TM is the tangent bundle of M, be in

18

19

the local coordinates;

n . a . a
X = ( A‘-—:- + B‘-—-—,- ),
E; 34‘ 3p‘
then
ixfldqi = Ai
€15,611)i = 3‘.
and for w2
in“? = 2&qu" A dp‘) = 2‘. < (iqu‘wp‘ - (ix,dp‘>dq‘ )
i=1 i=1
= 2(A‘dpi—Bidq‘)=dH.
i=1
This equals
" 8H . 8H -
d” = ( -—:d4‘ + —='dp‘ )
3i 3q‘ 3p‘
if and only if Ai = $ and Bi = — 311. Therefore
P

X: (are: ai_a1rai)_
i=1 81234 aq‘ap

 

The formula for X H is thus given by

a_”._
8p'

0 I
where J = {—1 J.

H
XH=( g—q)=JgradH

(3.3)

(3.4)

(3.5)

(3.6)

(3.7)

(3.8)

20

Thus, the Hamiltonian equations are

4" = 3'1 (3.9)
312

«My.
aqi'

This is a local representation of the Hamiltonian vector field X H defined by means of

w2 in the natural coordinates which describe the behavior of the system.

The free Hamiltonian function of a general n degrees of freedom mechanical
manipulator is obtained by deriving the kinetic energy, denoted by T, and the potential

energy, denoted by V, as follows. The kinetic energy is given by

T0: . d) = gimme. (3.10)

where q and q are the rod angular (or linear) position and velocity, respectively. M(q)
is the nxn inertial matrix which is known to be symmetric and positive definite, and

hence it is invertible. The generalized momentum p is defined as follows [34]

T
P = [37;] = M((M- (3.11)
34

Thus the kinetic energy may equivalently be expressed as

T = g. M(q)-1p. (3.12)

Thus the free Hamiltonian is defined as

Harp) = T (4p) + V(q) (3.13)

= é-pTMtqr 1p + We.

21

3.2 Hamiltonian Systems with Constraints

Now we place constraints on the free Hamiltonian system. The constraints
modify the equations of motion and generate new forces.

Let the 2m constraints be defined by the smooth functions

cw —) R, j= 1, 2m as
(19(q, p) = 0, j = 1, ..., Zn: (3.14)

Assume that 0 is a regular point of (3.14), see [31], then the constraint "surface" is a

2n—2m submanifold of M given by

C = [(q, p)e PM I <r>1(q, p)=,....,=<I>2”'(q, p) = 0 1 (3.15).

Furthermore, we require that the square (2m x 2m) matrix defined by
CM = [(6’)] = W. «>01 (3.16)

be nonsingular at every point on C, where { , } denotes the Poisson bracket. Eqns
(3.15, 3.16) in fact specify the class of allowable constraint surfaces in this formula-

tion.

The submanifold C is a symplectic (sub-)manifold with the induced symplectic
form WZIC, which is the restriction of the symplectic form w2 to C. Recall that the
Hamiltonian vector fields on a symplectic manifold form a Lie algebra under the Pois-

son bracket operation.
To satisfy the constrained Hamiltonian system, it is necessary that a correspond-

ing force of constraint be added to the system in the sense of the d’Alembert’s princi-

ple [see 22]. Let Q : M —> 7"‘M be the resultant force, then motion is given by

22
ixuw" = (111 + Q (3.17)

d<r>(X,,) = 0, j=1,..,2m,

2m .
where Q = 21,40 and it]- is the Lagrangian multiplier which in turn is proportional to
j=l

the amplitude of the force along the gradient of the j-th constraint function evaluated at

point (q, p), see [15]. In local coordinates, ixflw2 is given by

."aH-aH-han-hacbi-
l u = 2 ( --.-dq‘ + -—.dp‘ + A—dq‘ + A—dp‘) (3.18).
x i=1 34‘ 310‘ jg" 4' £131).
Therefore
X” = J grad HA, (3.19)
where
7»! .
Hy = H + 27c]- <l>’(q, p) (3.20).
j=1
Thus the equations of motion are (i = l, ..., n)
.1 3H 2’" 3(1)i .
q = -—. + Zl-—. (3-211)
319‘ i=1 1 at"
. 2m '
p.‘ = --afl. - 29192.,- i=1,...,n (3.2lii)
34' j=1 34'
dd) = 0 j=1,...,2m (3.21iii).

The geometric meaning of (3.21111) is as follows: let 7: t —>y(t) = (qi(t), p"(t)) be on M,

then the motion satisfies the constrains if d<I>i = 0. Also we rewrite (3.21iii) as

an, . .
0 = 3):;- = <D’(q, p) . (3.21“!)

By the symplectic orthogonal decomposition [33], the tangent space T*M can be

split with respect to the constraint surface C as shown in Figure 3.1;

23

T1”*Mx = TCJr O (TCI)L for every 16 C. (3.22)

 

 

Figure 3.1. The symplectic orthogonal space

Note that the the Hamiltonian vector field X H is generally not tangent to C.

The following two Lemmas are available in the literature. We reproduce them

here in a form which is appropriate to our subsequent results.

LEMMA 1 (13.171) : Let the Lagrangian multipliers be the vector i.=[i.,,....,i.z,,,]T. If
A. = Cab CH <1» (3.23)
where cm, =[ { H,<l>1 l ,...,[H,<l>2’" } F, and C“, is the (2m><Zm) matrix given in

(3.16), then the vector field of the constrained Hamiltonian is the vector field of the

free Hamiltonian restricted to the constraint submanifold C, namely,

X”). = XHIC (3.24)

24
Proof: XH. must be tangent to C. Thus, we have for i = 1, ..., 2m

. 2m . .
0: {HMCD‘} =[H+Zkfl>’,<l>‘}.
j=l
The Poisson bracket operation is bilinear and skew symmetric, consequently one

obtains

{H,<I>‘]=23.j{<b‘,<l>’}.
j=l

Since the matrix [ { <I>",<I>i ] ] is nonsingular, the lemma follows.
LEMMA 2 ([3]) : The vector field orthogonal to X [M is given by

2m .
(ch)1= 21,49 (3.25)
i=1

where (V, j = 1, ..., 2m are vector fields associated with the constraint functions

<D’(q,p), j=l,...,2m.

Proof: Since, for all j, (Di is zero on C, its corresponding vector field restricted to C
(i.e., (V l C) is zero. Consequently, (pi, j=l,...,2m, form a basis for (TC)'; the orthogonal

complement to the tangent bundle of the submanifold C.

25

The Lemma 1 and 2 can be graphically shown in Figure 3.2.

 

 

 

 

 

 

Figure 3.2. The decomposition of Hamiltonian vector X H

CHAPTER 4

THE CONTROL OF CONSTRAINED HAMILTONIAN SYSTEMS

4.1 The Control of The Contact Force

For a given set of constraint equations and under the conditions of Lemma 1, the
Lagrangian multiplier vector (k) can be determined uniquely by (3.12). In turn, the
corresponding (natural) contact forces are also determined at every point (q, p). These
contact forces are the natural response of the surface to (the end effector of) the mani-

pulator.

Here our focus and interest is in the m: we want to specify a desired
Lagrangian multiplier vector (1*), and consequently specify the (desired) normal force
at the point of contact on the submanifold C. Our viewpoint is motivated by applica-
tions, e.g., cutting, or grinding, etc., where it is desired to achieve contact at a specific

(normal) force.

Let the desired constrained Hamiltonian system (M, wz, H, CA.) be

27: ,
112144.12) = H(q.p) + 21*,- <D’(q.p). (4.1)
j=1

where 1*], j=l,...,2m, are any desired functions of q and p.

We choose functions Gimp) , j=l,...,2r for some integer r, defined on T*M with

the following properties

(1) ®i(q,p) = 0, j=l,...,2r (4.2)

26

27

(ii) the differential l-forms (19’ are linearly independent at every point on (equivalently

0 is a regular point)
B = [(q.p)e PM I 91(qp)=....=92'(q.p)=0}. (43)

where E is the 2n-2r smoome embedded submanifold of T*M. Then E is a symplec-

tic manifold with symplectic form wzlE.
(iii) The square matrix

E... = [(901 = [19" . 9'11 (M)

is nonsingular at every point on E.

The conditions (4.3) and (4.4) specify the class of E surfaces allowable by the
formulation. It is now required that the submanifold C (Zn-2m dimensional) be embed-
ded into the submanifold E (Zn—2r dimensional) or vise versa. For the case of m<r the
orthogonal submanifold of C must be a subset of the orthogonal submanifold of E,
consequently we can choose any 1* defined on C. On the other hand, the control sur-
face E is a subset of the constraint surface C, consequently the flexibility of steering
the position and velocity on C is restricted to the subset E. For m>r the converse is

true.

The Hamiltonian of this new constrained (Hamiltonian) system is

2' .
11,, = 11,. + 212,91. (4.5)
i=1

As in section 2.2, the tangent space of T*M can be split at every point on the

submanifold E as follows

WM, = TE, (9 (TE,)L.
Then the Hamiltonian vector field X H” and X”, have the decompositions

301,1me = XH,.(Cx) 9 (XH10(CX))IL

28
x,,r(r*M), = XH'(E,) o (29,413,»L , (4.6)

where X H”(Cx)e TC, and X ”’6 T Ex

LEMMA 3:
XH10(CX) = XleC, (4'7)

XH,(Ex) = XHPIE,

Proof: For any vector v 6 TC, we have
w2(Xu,.(C.).V) = W2(Xn,.(77*Mx).V) — w2( (Xn,.(C.»'—.v)

= <de¢,V> = (dHNulCJ’), fOI' all X.

Similarly, for v‘ 6 TE,.

LEMMA 4: Suppose that the input vector is given as

it = 5519 EH9, (4.8)

where 11:02,, ..... ,agJT, 1599 is as defined in (3.4), and

EH9 =[ {H1331 1 , ..... , [111.9% I 17.

Then the vector field of the total Hamiltonian is the vector field of the desired

constrained Hamiltonian restricted to E, specifically,

XH, = XH,.IE - (49)
Moreover, the orthogonal vector field to X11145 is given by

2r .
(XH‘_|E)L= 212191, (4.10)
j=1

29

where 0’ is the vector field associated with 9(qp)

Proof: Along E, we have for all o‘,i=l,...,2r

. 7! . .
0 = (H,,,e') = (11.. + zapaew
i=1

. 2r . .
= I HWO‘ 1 — 2a,. 1 one! 1 .
j=l

Consequently, if { 9&9" } is nonsingular, eqn. (4.8) follows.
We now summarize our results in the following theorem.

THEOREM 1: Let r be equal to m and let the input constraint equations be identically
zero on C. For a desired 33" defined on C, which corresponds to a desired (normal)

force, the corresponding input control vector is given by (4.8)

Proof. Since the submanifolds E and C (see Figure 4.1) are now identical, the vector
field generated by the input constraint equations affects only the directions normal to C

(Lemma 3). The rest of the proof follows from the previous lemma.

 

 

 

 

Figure 4.1. The (constraint) surfaces C and E

30

Now we consider a geometric interpretation of the force control. By the effect of

2m .
the input ii, the Hamiltonian energy level is changed from H to H + Zrifb’ as shown
j=l
Figure 4.2. The decomposition of the new Hamiltonian vector field can be presented

on the constraint surface as before. The orthogonal vector of the new Hamiltonian vec-

tor field is corresponding to the desired Lagrangian multiplier vector.

2m .
Ht—i =H ‘1" 2121-4”

 

 

 

 

 

i=1
Xugtc
\
\
\
\
\\
K
it
'= X11- ‘
' l
(XII;IC)‘ ';

 

Figure 4.2. The decomposition of the constraint Hamiltonian vector

Remark: When the submanifolds E and C are identical, the expression of control (4.8)

simplifies to

u“

CH4) '- A,“

€331 { 111.91 1 , . . . , { H1419": 1 1T = vac”... - (3.3.111
Cots

This simpler expression simplifies the computations considerably.

31

4.2 The Control of Compliant Motion

The end effector is acted upon by the input expression given in Theorem 1 to
exert the desired contact force at any point on the constraint surface. Now we consider
the scenario where it is desired that the end effector moves along a specified path on
the constraint surface, while maintaining a desired contact force profile. This scenario
describes the so-called compliant motion control. To develop a control that achieves
compliance, we need to augment the previous force control by a position (and a velo-

city) control onto the constraint surface.

Construct the functions ‘I’j(q,p) : T"‘M —9 R j=l,...,21 such that ‘Pj(q,p)=0 is
defined. Then as before the Hamiltonian for the augmented constrained system is given

by

21 .
HT<41P.Ll7117)= H,2 + Zaj‘l’l. (4.11)
i=1

where (a,,....,tr2,) is another input control vector which directs or steers the vector
field on the constraint surface C.

We know that H‘2 is a constrained Hamiltonian which has 1* on C maintained by
the input 12‘. The constraint equations (‘I’j(q,p)=0) must be chosen such that the input it
affects only directions tangent to C. Thus we define the submanifold

S = {(4.106 we I ‘P‘(q.p)=-..=‘I’2’(q.p)=0"}. (4.12)

where the differential 1-forms of ‘I’j(q,p) are linearly independent on s (i.e., 0 is a reg-

ular point) and

SW)? = [(‘1’01 =[ { ‘1". ‘1’} l l (4.13)

32

is nonsingular at every point on S. Then S is a symplectic manifold with the symplec-
tic forrn wzlS. S must be of co-dimension 2n—2m (i.e. the dimension of S must equal
to the co-dimension of C) and be a submanifold transversal to C as (at every point

along the desired path on C) shown in Figure 4.3. Hence l=n—m.

S (2n - 2m)

 

 

 

l
I
C(Zm) l
I
I

Figure 4.3. The (constraint) surfaces C and S

We consequently obtain the following lemma

LEMMA 5: Let the desired Hamiltonian for the compliant motion on C be H*. If 1‘2 is

given by
where tr=[a‘, . . . ,ah‘mlT, SW = [(119)] =[(\1"',‘11111 and

S,N=[{H-H*,‘I"},...,{H-H*,‘I’2"‘2’”}]T

33

Then the Hamiltonian vector field of H becomes identically the Hamiltonian vector

field of H*.

Proof: On S ‘I’j(q,p) = 0. At every point on the constraint surface C, the tangent to the
compliant control surface S, namely TS, is orthogonal to TC. Thus d‘I’j j=l,...,21, form
a basis of TC. Along a given path on C, the following condition is satisfied for all

‘11‘(q,p), i=1,....,2n—-2m,
{H*9 \Pi} = (HT, Vi]
. 2m . . 21 . .
= {H, 3"} + {2(37 '1" 12ij, W'} + (21713”, ‘1‘“).
j=1 i=1
The second term on the right-hand side vanishes since the Poisson bracket {49 , ‘Pi ]

is always zero for all indices. Hence

2n-2m . . .
“l X fij‘I’J,‘P‘} = lH—H*.‘P‘l. or
i=1
2 FAWN”) = {H-H*.‘I"l.
1:1

which gives equation (4.14).

Remark:

Note that the control input ii modifies the total Hamiltonian HT so that the desired

path on C is an integral curve of the resulting Hamiltonian H*.

Moreover, the orthogonal vector field to TS is given by

2n—2m ,
(anr= 2‘. W. (4.15)
j=1

34

We now summarize this result in the form of a theorem.

THEOREM 2: On C, the Hamiltonian HT has the desired Lagrangian multipliers and

its trajectories are steered along a desired path on C, provided that the inputs fi and ii

are given by (4.8) and (4.14) respectively, (see Figure 4.4).

 

 

 

Figure 4.4. The force control a and the motion control it

CHAPTER 5

THE REQUIRED TORQUE INPUT

The control expressions we have derived must be generated by the input torque
at the joints of the manipulator, otherwise the controls we have derived can not be
applied in practice. In this section, we show that it is indeed possible to generate the

proposed control expressions from the input torques.

The Hamiltonian of the overall compliant system (HT) is given by

HT: H+Zotj +u)<b’+ 22m uj‘PJ, (5.1)
j=l

and the corresponding differential equations are

2»: 2n—2Jn
+<ZO~1+ upg+ 2 1‘ng i=1 .. ...n (5.21)
F1 j=l 3p
.° 2»: 2n—2m j ..
pl = "_ _ 20‘ j+ “9W" "“2 Ufa—‘1' i=1...,n (5.211)
a: F1 ul)aqi j=l aq‘

The vector form of (5.2) can be written as

q = g—g + A1(u“ + A) + Blii (5,3i)
. aH . ._ ..
p = —8—q - A2(u + 9») — 32“ (5.311)

35

where

 

 

 

7.7--

36

 

 

 

 

 

_a<b?'"._' E
3!)1 aql

. A2 _
W" .32:
3P" J _ aq"
81’2”“ 8‘1”
3P1 dq1

. 32 _ .

aim—2’" 8‘!“
8p" . _ aq"

 

 

 

 

 

 

 

(5.4)

37

5.1 Special constraint functions

In this case (D and ‘1’ are not functions of p; they are functions only of the posi-
tion q. This corresponds to a physical constraint on the position alone, e.g. surfaces,

obstacles, etc.. In this case, equation (4.3) specializes to

- = 3%. (5.51)
l5 = -:—’Z — A202 + A) - th1 (5.5111

Consequently, the expression for the required physical torque can be read immediately

from (5.5ii) as

T = —A2u" -' 8217 (5.6)

38

5.2 General Constraint functions

We allow the most general constraint functions for which there corresponds an
input torque at the manipulator’s joints. The derivations proceed as follows. From

(5.3i), after multiplying through by M(q), we get

mm = p + M(thm + 3) + M(q)Btl'1- (5.7)

Now take the derivative of (5.7) with respect to time and substitute for p from (5.3ii).

Then, re-arranging terms, we get

M(qk'i + M(qlé + g]; + 91’-
q 34

= (—A2 + M(q)/1. + M(q)/1'02» + M(q)/113

+ <-A2 + M(q)/1. + M(q)/1'01: + M(qldrzi

+ (.3, + Mold. + M(qiérla + M(qlsrz‘i (5.8)
Eqn. (5.8) represents the 2nd order dynamic equations of the 2n-degree of freedom
manipulator. The left-hand side of (5.8) represent the model of the manipulator. On the
right-hand side, the first two terms containing 3 and 3. are due to the presence of the
constraint surface, while all of the subsequent terms ought to be generated by the phy-
sical torque at the joints. Therefore, we specify the physical torque at the joints for-
mally by

r = (—.«12 + M(q)/11 + M(q)/ion + M(q)A1r’i
+ (.3, + M(qlAr + M(qiéoa + M<q>Brii (5.9)

Note that the dots over the expressions in (4.9) denote differentiation with respect to

time. In the special case when the constraint and its orthogonal complement constraint

39

equations are not functions of p, i.e.,

<D(q) = 0

‘P(q) = 0.
Then A1 = Bl = A, = E, = 0. Hence the torque expression specializes to the case in
subsection (5.1), namely,

1: = “A212 -' 3211 (5.10)

 

CHAPTER 6

APPLICATION OF THE CONTROL LAWS
TO EXAMPLE MODELS

The applications are performed on the industrial three-joint revolute manipulator
shown in Figure 6.1; this type of manipulator is used in industry. The joint axes for
this manipulator are derived from the Denavit - Hartenberg specification [2] which is
an efficient formulation of the forward and inverse kinematics. The approach assumes

the model is a series of rigid bodies.

 

 

 

 

 

 

 

 

/

2:

Figure 6.1. A 3 degree of freedom manipulator

The kinematic energy of each link can be easily evaluated as follows [35]

40

41
_ 1 1 — P - P — P
T,_3m,v§,,,,+3(lz “guy? agar/Z wé) (6.1)

where m,- is the mass of link 1', Va,” is the velocity of link i at the center of mass, and
f), 5’} and 2} are, respectively, distances from the 1:14, y,-_1 and 2H to the center of mass

[see Figure 6.2]. The variables I; P, T,— P and I; P denote the principle inertia momenta

at the center of mass along the instantaneous directions it}, )7,- and 2}. The variables

wf, W7 and W7: denote the respective angular velocities along the instantaneous direc-

tions )7), )7,- and z‘, relative to the inertial reference space denoted by x0, yo and 20.

 

 

 

Figure 6.2. Physical constants of link i

The kinetic energy of the manipulator is given by

T = é— 'TM(q)ri. (6.2)

42

where

F'll(4) 0 o
M (q) = 0 r22(q) r23(4)
0 '23(4) “3(4).

(6.3)

 

 

and the the elements of M(q) are given by
rntq) = a. + as<sin(qZ»2 + altsintq2 + q’»2 — 2a5cos(r12>sitt(q2 + 43)
’22(4) = a7 — Zassintq’)
r23(q) = a8 — assin(q3)
’33(4) = 08-

The above constant values are given by

03 =01 -m23%—m31%
a4=m3s§+a2

05:"!2’253
ag=m25§+m31§+TfiP+i§zP+ffiP
a7=mzsg+m33§+mzlg+fizP+faP

08 = "133% + 7—2.3 P.

The kinetic energy as a function of p is described as

43

l _
T = —pTM(q) 1:2

 

 

2
— 1 l 2 l 2 2 3 2
_ 261mm + 2414(4) "33‘4““ + ’24P) (6.4)
- 2r22.(q)(p2)(p3)]

where r 44(4) = r 22(4)’33(4) — r 23(4)2-

And the potential energy is given by

V = (31ml + Ilmz + 12m3)g + alosin(q2) + allsin(q2 + q3) (6.5)

where alo=(szm2 + 12m3)g and au=s3m3g. Two examples are now provided to illustrate
the actions of the control laws when the manipulator is subjected to certain constraints.
In the first example, we choose the simplest (from the viewpoint of computation) con-
straint equations, and their complement orthogonal constraint equations; the constraint
are along the natural coordinates. In the second example, we illustrate the construction
of the complement orthogonal constraint equations for a practical constraint equations.

The two examples illustrate the actions of the introduced control laws.

Example 1:

Let the constraint equations be

«>1 = 42 (6.6)

then the complement orthogonal constraint equations are chosen to be the rest of the

coordinates, i.e. (q1, q3, p1, p3). Specifically, the orthogonal constraints are

\Pl __ ql
‘1’2 = q3
‘1’3 ___ pl (67)
‘1’“ = p3.

where the above constraints satisfy the following equalities

[oi .1101 =0 i=1,2 and j=1,2,3,4

and SW. is nonsingular.

The matrix C“, and SW are given by

 

 

0 1

CW: —1 0
O 0 1 0
0 0 0 1

SW: -—1 0 0 0' (6'8)
0 -1 0 0

The total Hamiltonian is given by
HT=H+(31+u‘1)<I>1+(32+122)<D2 (6.9)

+ apt" + 122‘112 + £191” + 174‘1’4.

Hence the corresponding dynamic equations are given by

45

 

 

 

 

 

 

 

 

a!
19p1
5 1! EL:- . . Al + 131
42 8p 0 0 0 0 l 0 + .
q 311 0 1 0 0 0 0 M “2
q3 3113 0 0 0 0 01 at (610)
= + ,.., -
p1 ___afif O 0 -1 0 0 0 “,2
p-z 3q -1 0 O 0 O 0 1‘13
p.3 _ £11 0 0 0 -1 0 0‘ ~
. . 342 “4
-1151.
. 34’.
Let the desired Hamiltonian for force control be
H1. = H + lt,*<l>‘ + we? (6.11)
Then, the force control inputs are calculated according to formula (4.8) as
121 l [ H1. , (D1 ]
= C‘ 6.12)
122 M I ”3* 9 (D2 1 (
— I H 9 (b2 i - A'1’"

{H.991 ’32“,

and the control inputs for the compliant motion (on the constraint submanifold) are

calculated according to formula (4.14) as

51'

 

 

 

 

"(H-Hard]-
122 -S *1 ”FIFTY“ (613)
123 ' W {H—H*,‘P3} '
a, .{H—H*.‘I’4l.

46

;{H-H*"¥3].
—[H—H*,‘I’4]

[H—H*,‘P1]
.1H-H*.‘P21.

 

 

Thus the augmented control system equations are "modified" to

 

 

 

 

-1 = 611*
8p1
12 = we
‘3 = g”: (6.14)
P
.1 ___ aH*
aq1
[52 = —(A-1—7\1*)
.3 = BH“
an3 '

We drive the following relation for the Poisson bracket;
{{H,F}G.Ql=G{{H.Fl.Ql (6.15)
+ {H,F} {G,Q}.
proof: By definition of Poisson bracket

{{H.F}G.Ql

=Z-:({H FIG)— 39. “rim H.—-F}G) 39

p134' ap‘ 84‘
" 3 8Q+ aGaQ
= -— H,F G— H,F -—.--—.
1:213Q‘H }) +{ }8q‘ap'

8_G_ LQ

8 8Q_
,.(IH F})G— {H F}
3‘4 8p 84‘

8P

47

:01 {H.Fl .Q} + {H,F} lG.Ql
We now take the Poisson bracket of (in (6.9)) HT and (D1. For the (HT, (1)1 }
= 0, we have the following relation;
0: {H,cb‘)
+0‘1-Nkl){¢la¢li - { {H.991 ¢1r¢ll
+(3q—3*2){<I>2,<I>l}+ ( {H,o11¢2,<r>11
- ( [H-H*,‘I’3} when
— l {H-H*,‘I’4} ‘l‘2,<l>‘} (6.16)
+1 [H—H*,‘P1}‘i’3,<l>1}
+1 {H—H*,‘I’2}‘I’4,<I>1]
Using the relation of (6.15), (6.16) results in
39:332-
Similarly, we obtain 31: 3*] from the requirement [HT , (1)2} = 0.

Observe that from 31 = 3*1 and 3.2 = 3*»), (6.14) reduces to

 

 

 

.1=3H*
8p1
42 = o
‘3 = am (6.17)
8p3
fi1=_aH*
aql
:52 = o
3 _3H*

 

9

48

which represents the dynamics on the constraint surface along the desired path.

Example 2:

Now we consider a practical constraint: if joint 2 is fixed and the rest of the joints are
allowed to move. Then the constraint surface (or region) becomes the inside of a donut
shape with its center located at (0.0.11) as shown in Figure 6.3.

A
Z

 

 

 

 

 

 

 

Figure 6.3. The constraint surface of Example 2

This constraint results in the equation

Eqn. (6.18) implies that q'2 = 0, which in the (q, p)-coordinates reads

'33(q)P2 - r23(q)p3 = o. (6.19)

49

Thus the constraint functions are
(1)1 = 42 (6.20)
(Dz = ’33(4)P2 " '23(4)P3-

The complement orthogonal constraint functions ‘1’], ‘1‘2, ‘1’3 and ‘I’4 are to be con-

veniently defined.

Consider the differential of the constraint equations and their orthogonal-

complement constraint equations.

 

 

 

 

le-
axi
3oz -
axi 0 1 0 0 0 0
aqn 00 b10192 b3
31! 1 0 0 0 0 0 621
’"axp2=0c2c300c5’ (‘)
axi 0 0 0 1 0 0
8T3 0 C7 C8 0 0 C10
ax}
.81":
.84.
where we used xl=q1,x2=q2, x3=q3x4wlgt5=p25r6=p3 and where
bl = ascos(q3)p3
b2 =08 (6.22)

b3 = —(a8 - a5s1n(q3)) .
Now given bj’s, any quantities ci’s which satisfy the equations
b1C5 — b2C2 — b3C3 = 0

blclo "' b2C7 —' b3C8 = O, (623)

50

would define an appmpriate complement orthogonal constraint equations. Conse-

quently, the following relations are satisfied
{ oi , ‘111' 1 = 0 i=l,2 and j=l,2,3,4.
The condition that St“. is nonsingular is given by
c3c10 — C5C8 at 0. (624)

Now we may choose the cj’s as follows
C2 = “—b3 C3 = b2
C7 3 b1 C10 = b2 .

Hence the matrix Cm and SW. are given by

0 b2
CW: —b2 0

 

 

'0 0 10'

0 0 0 63
SW: _1 0 0 0 (6.25)
’ _0 —b§ 0 0.

The dynamic equations now read

N'Ué-qb-Q'b-QLJ

 

 

”u. 'U.
Pt»

 

 

 

do1 -1 0 0
_8_H_ 0 “b1 0
842 '
__aa
. 3431

where the force control inputs are

ii1

112

=04);

{H3*’<p1}
{H3*r¢2}

 

5|

0 1 0' (31+121
0 0 0 3o+112
0 0 b2 12,
0 O 0 122
b3 0-61 :13
—b20 01 a4

 

 

 

LLIH3¢2}_A1*

1’2

= i 1-
bleJD} M"

and the compliant motion control inputs are

is‘

 

 

= SW

P

 

-l

 

-[H-H*,‘I’3]

-i, {II-Hare}
b2

{H—H*,‘I’l}

i {H-H*.‘I’2}
bi

l{H—H*,‘I’1}-
{H—H*,‘I’2}
{H—H*,‘I’3}
.[H-H*.‘I’4l.

 

 

 

1

(6.26)

(6.27)

(6.28)

52

The augmented control system thus becomes

 

 

 

 

(6.29.i)

(6.29.ii)

(6.29.iii)

(6.29.iv)

(6.29.v)

(6.29.vi)

.1 = 311*
312‘
42 = 1720442")
.3 811* b3 aH*
= -— + b * — —-—-
q 3p3 30‘2“ A’2 ) 223p
=-%—*”, + 19301-12”
fil : _ 8H*
Bql
311* __b_l_ am
= —(x 4» *)+ b3
p21 1 52 8q3 b2 8p3
8H*
= -(3. - 3. * — —
1 1 ) 8q2
.3 311* _1_b _3__H*
= -— — b —3 *
P 3q3 101-2 2 )+ b—z- apz
BH“
= -— - b ( - *.
where we have used the following relations for (6.29.iii),(6.29.v) and (6.29.vi), respec-
tively,
8H*
0 = H* , o1 = —,
l 1 ap,

BH“ BH’“ am
0: H*,<I>2 =b——+b——b .
{ I 2an 3an lap-1

 

As before, the Poisson bracket { HT , (D1 l = 0 results in

(6.30)

53
0: {H,cb‘}

+(31-3*1){<D1.¢'}--b1—{{H.¢21¢1.¢1}
2

+(3o-3*2){¢2.¢‘l +-bil {H.<D‘l <D2.<I>‘}
2

— [ [H—H*,‘I’3} 1111,61

--;15-[[H-H*,11141‘10,o11 (6.31)
2

+1 {H—H*,‘P1}‘I’3,<I>1}

+3131 {H—H*,‘P2} ‘114,<r>11
2

Using the relation of (6.15), the (6.31) becomes as
A'2 = 7U"2
and we obtain the 31: 3*1 from {H7~ , (D2) = 0.

From 31 = 34*, 3.2 = 31*, Eqns. (6.29) reduce to

 

 

 

 

.1 = 611*
Bpl
cf = 0
.3 3H*
= (6.32)
4 ap3
1 = _ 811*
8q1
2 z _ 3H*
aq2
. 8H*
123 = -

 

CHAPTER 7

STABILIZATION ON THE CONSTRAINT SURFACE
FOR THE EXAMPLE MODELS

Example 1

Let H be the Hamiltonian of the actual model and ii the mathematical Hamil-

tonian. Now choose the force inputs as

(7.1)

121 —{fi,¢2} -3.*1+e;_1+kp5(p2-p2.)+k;5e2
a2 = {174191-3*2+ei,-kp2(qz-42')-ktzez’

where ex; = 3,—3*,-, i=1,2 and 3. is the measured value from a force sensor. And

choose the control inputs for the compliant motion as

., ' 1 ~ . '
in, -{H,‘I’3}+kp4(pl—p1)+k,-4e4
- l H .11" l + 1W3 -p3‘) + knee
~ = " t 9 (7'2)
“3 l H 9 ‘1’] l ‘ kpl(ql "' ql ) " kilel

fir _ 111.11”) -k.3(q3—q3‘)-kaer

 

 

 

 

where
él = 41 - 4"
‘52 = 42 - 42.
e=r-r'
‘54 = P1 — P1.

54

55

és = P2 — P2.
é6 = P3 — P3:-
Then, the augmented control system including the control inputs (7.1) and (7.2) are
given by
(i1 = ' 161(41 - 41.) - kilel + m1
42=- p2(42-qr)-k1262+m2
43: " p3(43-43')-k13€3+m3
.1 = — k 1 — 1* — k.
P p4(P P ) 1434 + ”‘4

p2 = — kp5(p2 — p”) - Itge5 + m5 (7.3)

153 = - 1:1..er3 - p") - ki6e6 + m6

8.1: 41" Q"
‘52 = 42 — 42*
e=r-r*
‘54 =P1 -P1'
‘55 -'=P2 “P2.
36 =P3 “103‘.

where each m), i=l,..,6, is a mismatch function given as

= .311. _ 32.11

1 3p1 Bpl
= 2’1 _. 95.

2 an? ap2
8H 3H

56

at! at?
,= _ _ __ _ (7.4)
m4 (an aql)
,,, = _ (a_H _ 311.,
5 3q2 qu
m. _ _ (a_H _ .85.,

Let the Liapunov candidate function be

E = ilq‘ — .132 + é—(qz - 42*)2 + %(43 — q")2

2
11 11121221213 3*2

+-2-(p -p)+3(p -p)+3(p -p) (7.5)
l 1 l 1 1 l

+ ‘2‘kilei + "249233 + ‘Z'knei + ‘z'kittei + 3""583 + 3,9632»

where kik, k = l,..,6 are positive constant values. The derivative of E along the trajec-

tories is calculated to be
E = - kprtq‘ — 41:12 — .2012 — 42*)2 — .3013 — 43*)2
_ kp4(pl _ pl"')2 _ kp5(p2 _ p2*)2 _ kp6(p3 _p3“)2
+ "11(4‘ - q") + mz(q2 - q”) + ms(q3 - 43') (7.6)

+ "140‘ - p") + new2 - 102') + "16003 - 123')-

Over any compact region in the state space, the mismatch quantities mi, 1' = l,...,6
are bounded. Assume that each mi, i=1,...,6 is bound above by a (positive) constant

271,-, i.e. lm,l S Fig. Now consider the hypercube:

' 't if: i i E3“.
R= {(q.p)||q‘-q‘ls— .Ip -p*|S
kpr’ kp3+i

 

, i=1,2,3} (7.7)

Then outside the hypercube R, we have

57

E S _ kpl(q1 .— ql‘)2 _ p2(q2 — 4232 -' p3(q3 _ q3.)2 (78)
._ kp4(pl _ pl‘)2 _ kp5(P2 _ pZ‘)2 _ kp6(p3 _ p3‘)2 S 0
Consequently, all initial conditions of the system equation (7.3) converge to the hyper-

cube R defined by (7.7). Moreover, if m, a: kp,(q" - q‘*), "13+,- ¢ kp3+,-(pi - p“), i=1.2,3.
then E = 0 if and only if q‘ = q“ and pi = p", i=1,2.3.

58

Example 2

Choose the force inputs and the control inputs for the compliant motion as

 

 

 

 

 

 

, .
.. -bi{fi,<b2] —3.*1+e;[l+l21*
“l 2
= 7.9
“’2 in”: ol1—it*+e+**’ ()
b2 ’ 2 M “2
and
H 417.1113) +a,*
tr ..
‘ --L{H.‘¥‘} +1221!
~ b2 1
“2 — 2 (710)
it, " (11,1141) +1131: '
F4. J—lfiflfll +12;
bi
where

:1: b3 t
531* = k 5(P2 " P2 )+ ((1585 + _[kp6(p3 - P3 ) + [(1686]
P b2
b1 s
+ b—[kpsfil3 - 43 ) + ki3e3]
2

. 1 .

“2* = - Elkp2(q2 — (12 ) + kiZeZ]

i11* = kp4(Pl - P1.) + ((1434

17*‘iUc (p3— 3‘)+k.e1+fl[k (2— 2'")+It e]
2-b2 p6 P :66 b§ p24 (1 122

(23* = — [kpl(ql - ql‘) + knell

59

.. l t b3 i!
“4* = — "El/6,1343 " 43 ) + (61333] + b—glkpzfllz " 42 ) + [(1262]

l 1“

él =4 ‘4

(52:42—42:
e=r-r'
é4=P1-P1‘
é5=P2-P2*
é6=P3—P3‘-

The above linear control inputs (121*, 122*, 121*, 172*, 173*, if) can be obtained using the
Liapunov function approach; for a Liapunov candidate function such as (7.5), substi-
tute (7.9) and (7.10) into the derivative of E and then choose the linear control inputs
which would make E negative semidefinite. The augmented control system equations

using the control inputs (7.9) and (7.10) are given by
41 = "'1 + ‘73:“
.2 _ A *
(I - m2 + P2142
(13 = "'3 + 173122“ + “2174*
.1 _ __ *
P - m4 — “2

P2 = m5 - 171* + bsaz’“ " [9134*

P3 = ”'6 — [7132* “ b2172* (7.11)
él = 41 - 41*
6'2 = 42 - 42‘

60

o _ 2 t
e5 - P - P2
o _ 3 t
86 — p _ p3 9
where mi, i=l,..,6, is a mismatch function defined as before.

Let the Liapunov candidate function be as before and take the derivative along

trajectories. Then E becomes identically (7.8) under the same assumption (7.7).

CHAPTER 8

SIMULATION

Computer simulations are performed for Example (2) discussed earlier to examine
the effectiveness of the control algorithms introduced. The parameters of the manipu-
lator are set as in Table 1. For the computer simulation of the dynamic equations, a
fourth-order Runge-Kutta method is used on a VAX 11/780/8600 Ultrix computer sys-

tem.

 

Link Mass(Kg) Length(m) ff; P (Kgmz) TE- P (Kgmz) Tz—‘P (Kgmz)

 

 

 

1 15 0.8 3.2
2 10 0.7 0.41 1.63 1.63
3 7 0.5 0.21 0.84 0.84

 

 

 

 

 

 

 

 

Table 1. The physical parameters of the 3 d.o.f. manipulator

There are two possible ways to obtain the desired values; one is to obtain the
desired Hamiltonian for the given desired force and motion if possible, and the other is

to obtain the vector field corresponding to the desired Hamiltonian as

 

 

*
a; = 4* (8.1)
— as: = [5* = mg + My: . (8.2)

To simulate the performance of the robot manipulator, the desired path is chosen as

61

62

q“ = 0.02: (rad)

qr = 0.0 (rad)

q” = 0.01: (rad) ' (8.3)
q” = 0.02 (rad/sec)

42* = 0.0 (rad/sec)

()3. = 0.01 (rad/sec),

and the desired Lagrangian multipliers as
1*1 = 0.02 (8-4)
K’kz = 0.0

Figures (8.1-3) and Figures (8.4-6) depict the result of the control in achieving
position and velocity tracking, respectively, for the desired values with initial condition
set at (41:00, q2=0.0, q3=0.0, 41:0.02 42:00, 43:0.01). Figures (8.7-12) display the
corresponding torques (til, 122 fil,..,i24). Figure (8.13 and 8.14) show the resulting
Lagrangian multipliers.

In this simulation, the integration frequency is taken as f = 100,000. Even if the
initial conditions are the same as the desired values, we see a diversion of the trajec-
tory in Figure 8.4. This indicates that the overall system is unstable. To overcome the
instability effect of numerical errors, we must take advantage of feedback of error sig-

nals.

For the control inputs (7.9) and (7.10), Figures (8.15-26) depict the result of the
control in achieving position and velocity tracking and the corresponding torques with
the initial condition set at (q1=0.1, q2=0.0, q3=0.1, q'1=0.0 q°2=0.0, q'3=0.0). In this
simulation, the integration frequency is f = 60 and the feedback gains are kpk = 5.0 and
k”, = 5.0 k = l,..,6.

63

 

 

20

-----------L_---------.

i
10
time (sec)

----------%-----------
I
i
I
5

 

 

 

 

_-----------—---------

 

 

1
I
I
I
I
I
I
I
I
--_------_-J-----------
.

0

0 4
0.2
-0.2
0.4
0 2
-0.2

Figure 8.1. Plot of the (generalized) angle 41 versus time, Example 2
q
(rad)

20

time (sec)

 

Figure 8.2. Plot of the (generalized) angle q2 versus time, Example 2

64

 

 

""""""""

q

1----------

1
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I

----_---------_-------L--------_--

---_------J

""""""

""l

"-||-|-'l

 

 

0.2

0.1 ~---------—

(rad)

0

—0.1

20

time (see)

Figure 8.3. Plot of the (generalized) angle (13 versus time, Example 2

 

IIIIIII

""'I"'

 

I"I'--'|'|"""'-|l

 

1

-----------L----------.

h-------—--J-—---------

"""""""

l'lolullll-"l

 

 

0.04

0.02

(rad/sec)

0

-0.02

20

time (sec)

Figure 8.4. Plot of the (generalized) velocity q'1 versus time, Example 2

65

 

 

j
I
I
I
I
I
I
I
I
I

I-I

I
I
I
I
I
I
I
I
I
L

 

20

 

 

0.04

0.02»----------

'2

q
(rad/sec)

-0.02

time (sec)

Figure 8.5. Plot of the (generalized) velocity q'2 versus time, Example 2

 

 

I

I

-—-------—4

-----------L-_--_--_---

 

----_------J-----------

l

 

 

0.04

0.02»----—-----

0

(rad/sec)

-0.02

20

10
time (see)

Figure 8.6. Plot of the (generalized) velocity q'3 versus time, Example 2

66

 

 

20

  

I"I-'-"' """' "-"""l

---_---_---r_-----_---.
___-----__-L__----_--

i
I
I
10
time (sec)

1---------_-

IIIIIIIIIIIIIIIIIIIIIIIIIIII

 

 

 

 

 

 

_3_______,_-_
_9____-______
-10
0
0095
-0£5
0

Figure 8.7. Plot of the force control :21 versus time, Example 2
—0.025 ~---—------

20

Example 2

time (sec)

 

Figure 8.8. Plot of the force control If; versus time,

67

 

A

T
I
I
I
I
I
I
I
I

-I

 

l'I-‘"I'-A""'-‘-'-"|"""

j
I
I
I
I
I
I
I
I

 

20

I.

140

time (sec)

 

 

0.025

o.-------_-_

—0.025_----------

11 1
(NH!)

 

r

I

I

I

I

I

I

I

I

I
-------_---r-----_----.
--—-——--——-L-—--—----—q

1--------_--

1
I
I
I
I
I
I
I
I
I

 

p--—------—J—----————-—

"."1

1.3.4

 

20

15

 

 

Figure 8.9. Plot of the motion control til versus time, Example 2

0b----------
—1

ti 2
(N-m)

time (see)

Figure 8.10. Plot of the motion control rig versus time, Example 2

 

68

 

 

     

.1---..---..---

15 20

10

time (sec)

 

 

0.0025

-0.0025.---------—

—0.005
0

Figure 8.11. Plot of the motion control 113 versus time, Example 2

 

 

 

 

 

0
i u 2
. _
_ _
_ _
- _
. _
. _
. _
.
. _
- _
r IIIIIIIII r IIIIIIII I:— IIIIIIIIII w
. _
. _
- —
. _
. _
. _
. _
. _
. _
_ _
IIIIIIIII _IIIIIIIII.._IIIIIIIII 0
_u . _ L..I.
. _
. _
_
.
.
.
.
-
.
.
1 IIIIIIIII .I IIIIIIIIIIIIIIIIIII .5
.
.
.
.
.
.
.
.
.
-
b
O
5 5 O 5
0 0 0
e c c
5 5. 5.
2 2
\I _
.wm
m.

time (see)

Figure 8.12. Plot of the motion control I34 versus time, Example 2

69

 

 

----------d

"|l||""|ll"|"-l

 

 

 

0.03
002
0.01-----------

11

20

time (see)

Figure 8.13. Plot of the Lagrangian multiplier 7L1 versus time, Example 2

 

 

IIIIIIIIII

 

 

 

002
0.01-----------

12

20

10
time (sec)

—0.01

Figure 8.14. Plot of the Lagrangian multiplier 3.2 versus time, Example 2

7()

 

 

""'

"' """"

“""""T"""""‘

I

I

I

I

I

I

I

I
_-___-___-_L--_-------.

20

........

 

 

0.4

0.2-----------
0

-0.2

time (see)

Figure 8.15. Plot of the (generalized) angle 41 versus time,
Example 2 (the overall nonlinear control)

 

 

I

—---------—---—-------d

b----------------------

 

20

 

 

0.4

0.2

q
(rad)

—0.2
0

time (see)

Figure 8.16. Plot of the (generalized) angle q2 versus time,

Example 2 (the overall nonlinear control)

71

 

 

I'-'I I"'

""|"-"

----—~—----L---~------d

IIIIIIIIIIIIIIIIIIII

20

 

 

0.2

—0.1

time (sec)

Example 2 (the overall nonlinear control)

Figure 8.17. Plot of the (generalized) angle q3 versus time,

 

20

t
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I

-_-__---__-I.-_--__-___.
I
I
I
I
I
I
I
I
I
l
5

 

 

 

"""""'i""""”'
5

 

 

004

0.02- ---—
0

—0on

time (see)

Figure 8.18. Plot of the (generalized) velocity 4'1 versus time,

Example 2 (the overall nonlinear control)

72

 

 

I

I

 

20

"""I'll

 

 

0.04

-0.02

time (sec)

Figure 8.19. Plot of the (generalized) velocity q'2 versus time,

Example 2 (the overall nonlinear control)

 

 

1'---

'll'-

""'|"||

____-_-----L---_------.

 

.-------_--J----------_

20

 

 

0.04

0.02.--—--——-——

0

(rad/sec)

—0.02
0

time (see)

Figure 8.20. Plot of the (generalized) velocity q'3 versus time,

Example 2 (the overall nonlinear control)

73

 

IIIIIIIIII

"""""

"' .....

 

r-—-—-------

     

-_------_--L-------__

J-----------

15

20

10
time (sec)

 

 

-8

_9_-____-____

—10
0

Figure 8.21. Plot of the force control u“ 1 versus time,

Example 2 (the overall nonlinear control)

 

d

1
I
I
I
I
I
I
I
I
I

 

"""""""

20

 

 

 

5e-05
2.5e-05~---—___-_-

—2.5e-05

time (sec)

Figure 8.22. Plot of the force control 143 versus time,

Example 2 (the overall nonlinear control)

74

 

 

_____-----_L_----_-___-

I
I
I
I
I
I
I
I
I
1

 

 

 

20

10

time (see)

Figure 8.23. Plot of the motion control If 1 versus time,

Example 2 (the overall nonlinear control)

 

I'

I

I

I

I

I

I

I

I

I
--——-——-———r—-—--———-—q

1
I
I
I
I
I
I
I
I
I

 

0r----------

j
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
I
---—---—---'---—-------

L-----______

_----------J

I'-'-l

|'--'l

 

 

 

20

time (sec)

Example 2 (the overall nonlinear control)

Figure 8.24. Plot of the motion control :12 versus time,

 

75

 

 

 

 

_-__-__....--.L-..-.._-____..

IIIIIIIII

 

 

0.5

u" 3
(N-m)

—0.5L.-------—--
1

20

time (see)

Figure 8.25. Plot of the motion control u~ 3 versus time,

Example 2 (the overall nonlinear control)

 

1
0.5------—----

----------------------d

1
I
I
I
I
I
I
I
I
I

 

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

 

 

 

 

—0.5

20

time (sec)

Figure 8.26. Plot of the motion control 134 versus time,

Example 2 (the overall nonlinear control)

 

CHAPTER 9

SUMMARY, DISCUSSION AND CONCLUSIONS

The geometric tools of symplectic Hamiltonian systems are used to develop a
compliant control of constrained robot manipulators. To formulate the compliant con-
trol of constrained robot manipulators, we first analyze the geometric characteristics of
singular differential equations which represent the governing system of dynamic equa-
tions with algebraic constraints. The analysis employs a constrained symplectic Hamil-

tonian systems.

Based on the analysis, we propose a control strategy which consists of the sum of
two nonlinear controls, i.e., the force control part and the position conu'ol part. By
these two nonlinear controls, the desired force and the desired position trajectories can
be realized simultaneously, that is, the force control restricts (the end effector of) the
manipulator to the constraint surface and at the same time the position control steers
(the end effector of) the manipulator along a specified path on the constraint surface.
Such controls are possible because the vector field is decomposed into a normal and a

tangential components with respect to the constraints surface.

Consequently, we are able to control the normal component of the vector field at
every point on the constraint surface and thereby solely modify the force effect. Analo-
gously, we are able to control the tangential component of the vector field at the same
point on the constraint surface and solely modify the position or motion of the manipu-
lator (constrained to the constraint surface). The conditions resulting from our formula-
tion in fact specify the class of constraint surfaces allowable. The derived conditions
on the constraint surfaces are given in term of a matrix of Poisson brackets being non-

singular.

76

77

The effort in the formulation phase focuses on guidance and deep insights into
how to properly devise and apply the control strategies even if this formulation does
not include error signals of a desired position, velocity or force in order to counteract
the effect of unmodeled dynamics and disturbances. As a final phase, we applied a
linear controller to the resulting model to achieve attractivity of the constraint surface

and to take into account the effect of disturbances and unmodeled dynamics.

The force of contact is a function of the state of the system. However, it may
appear to qualitatively represent the dynamics of the contact force near the constraint

surface.

One approach to modeling the force may be to modify the algebraic constraint
equations to convert them to singularly perturbed differential equations. These equa-
tions act as a model for the dynamics of the contact force for when the end effector is
near or onto the constraint surface to account for the chattering behavior frequently
observed in applications. The basic feature of the model is that the rate of change of
the amplitude of the force at some point, normalized with respect to the force ampli-
tude itself, is (negatively) proportional to the algebraic constraint evaluated at that
point (8f = -<I> where 8 is small number). The physical meaning of this model is
explained as follows. The rate of change of the contact force is related to the con-
straint function. The end effector is acted upon by the input torques to exert force on
the surface. In response, the constraint surface generates a reaction force. The rate of
change of the force equals zero, thus having a constant force value, if the end effector
is positioned on the surface; it becomes positive, thus leading to an increase in the
force value, if the end effector were positioned beneath the surface; and it becomes
negative, thus leading to a decrease of the value of the force, were the end effector

positioned above the surface.

Improved models should be derived for the force near or in contact with the sur-

face based on phenomena arising between the end-effector of manipulator and its

78

environment, and it may also include the dynamics of the material of the constraint

surface itself.

After investigating the modeling problem, one may apply a controller, such as a
singular perturbed controller, which characterizes a smooth approach from and off the
constraint surface at the contact point. Then the manipulator may follow the desired
path on the constraint surface with the desired force trajectories without changing con-

trollers on the constraint surface.

Compliant control of constrained robot manipulators is well behind vision in both
theory and level of applications. So more effort is needed to identify and solve basic
theoretical problems that take into account the dynamic equations of the constrained

robot manipulator and the constraint equations.

On some of these points, this dissertation provides some insight into the dynamic
behavior of constrained robot manipulators as they relate to their environment, and
also it provides guidance onto how to simultaneously realize the desired position and

force trajectories onto a given constraint surface.

To be stabilized on the constraint surface, we have discussed the addition of a
linear controller to example models. This stabilization is valid only on the constraint
surface. Thus, there are other challenging issues, such as the development of more
sophisticated algorithms capable of stabilizing the constrained system near and onto the
constraint surface. Such algorithms may include the dynamics generated due to the

material of the constraint surface itself.

LIST OF REFERENCES

[1]
[2]

[3]

[4]

[5]

[6]

[7]

[8]

[9]

LIST OF REFERENCES

M.Brady et.al.(editor), "Robot Motion: Planning and Control," MIT Press, 1982.

R.P.C.Paul, "Robot Manipulators: Mathematics, Programming, and Control," MIT
Press, 1981.

N. Harris McClamroch and Anthony M.Bloch, "Control of Consuained Hamil-
tonian Systems and Applications to Control of Constrained Robots," Dynamical
Systems Approaches to Nonlinear Problems in Systems and Circuits edited by

FMA Salam and ML. Levi, SIAM, 1988, pp. 394-403.

N.Harris McClamroch and Danwei Wang, "Feedback Stabilization and Tracking
of Constrained Robots," preprint, Dept. of Aerospace Engineering, University of
Michigan, 1987.

M.H.Raibert and JJ. Craig, "Hybrid Position/Force Control of Manipulators,"
ASME J. of Dynamic, Systems, Measurement and Control, 102, 1981, 126-133.
O.Khatib, "A unified Approach for Motion and Force Control of Robot Manipula-
tors: The Operational Space Forrnulation," IEEE J. of Robotics and Automation."
Vol. RA-3 Feb. 1987, pp. 43-53.

C.S.Yoon and F.M.A.Salam,"Compliant Stabilization Control of Constrained
Robot Manipulators," IEEE Conference on Systems, Man, and Cybernetics, Alex-
andria, Virginia, 1987, pp. 903 -905 .

M.Takegaki and S.Arimoto, "A New Feedback Method for Dynamic Control of
Manipulators," J. of Dynamic Systems, Measurement, and Control Vol.102 June

1981, pp. 119-125.

M.Takegaki and S.Arimoto, "Stability and Robusmess of PID Feedback Control

for Robot Manipulators of Sensory Capability," Robotics Research, MBrady and

79

80

RPauI (editors), MIT Press, 1984, 783-799.

[10] F.M.A.Salam, "On The Design and Control of Robot Manipulators," IEEE Sys-
tem, Man, and Cybernetics, Oct. 14-17, 1986, pp.l95-199.

[11] F.M.A.Salam, "Velocity feedback Control and Dynamic Properties of Robot .
Manipulators," The 26th IEEE Conference on Decision and Control, Los Angeles,
December 9-11, 1987.

[12] N.Harris McClamroch, "Singular Systems of Differential Equations as Dynamic
Models for Constrained robot Systems," RSD-TR-2-86 University of Michigan.
Los Angeles, December 9-11, 1987.

[13] W.C.Rheinboldt, "Differential - Algebraic Systems as Differential Equations on
Manifolds," Mathematics of Computation, 43,1984, 4 73 -482.

[14] S.L.Campbell, "Singular Systems of Differential Equations, 11," San Francisco,
Pitman, 1982.

[15] R.W.Weber, "Hamiltonian Systems with Constraints and their Meaning in

Mechanics," Archive for Rational Mechanics and Analysis, 91 , 1986, pp. 309-335.

[16] J-C. van der Meer and R.Cushman, "Constrained normalization of Hamiltonian
systems and perturbed Keplerian motion," J .of Applied Mathematics and Physics
(ZAMP), V0137, May 1986.

[17] P. Deift, F. Lund and E. Trubowitz, "Nonlinear Wave Equations and Constrained
Harmonic Motion," Commun. Math. Phys. 74. pp. 141-188, 1980.

[18] SH. Drake, and SN. Simunovic, "The Using Compliance in a Robot Assembly
System," IFAC Symp. on Info. and Control Problems in Manuf. Tech., Tokyo,
1977.

[19] RC. Watson, "A multidimensional System Analysis of the Assembly Process as
Performed by a Manipulator," 1st N. Am. Robot Conf., Chicago, 1976.

8]

[20] J.l(. Salisbury, "Active Stiffness Control of a Manipulator in Cartesian Coordi-
nates," IEEE Conf. Dicist'on and Control, Albuqerquerque, New Mexico,
November, 1980.

[21] J.K. Salisbury, and JJ. Craig, "Articulated Hands: Force Control and Kinamatic
Issues," Int. J. Robotics Research, Vol. I, No. I, 1982, pp. 4-17.
[22] H. Habafusa and H. Asada, "Stable Prehension of Objects by the Robot Hand

Elastic Fingers," Proc. 7th Int. Symp. Industrial Robots, Tokyo, October, 1977,
pp. 361 -368.

[23] D. Siver, "The Little Robot System," Artificial Intelligence Laboratory, Mas-
sachusetts Institute of Technology, AIM 273, January, 1973.

[24] DE. Whitney, "Force Feedback Control of Manipulator Fine Motions," J.
Dynamic System, Measurement, Control, June, 1977, pp. 91-97

[25] T. Goto, T. Inoyama, and J. Takeyasu, "Precise Insert Operation by Tactile Con-
trolled Robot," 2nd Conf. Industrial Robot Technology, March, 1974

[26] N. Hogan, "Control of Mechanical Impedance of Prosthetic Arms," Proc. 1980
JACC, San Francisco.

[27] IL. Newins and DE. Whitney, "The Force vector Assembler concept," Proceed-

ing, First CISM-IFTOMM Symposium on Theory and Practice of Robots and

Manipulators, Sep. 1973.

[28] RP. Paul, "Modelling, Trajectory Calculation, and Servoing of a Computer Con-
trolled Arm," Staford University, Artificial Intelligence Laboratory, AIM 177,
November, 1972.

[29] RR. Paul, and B. Shimano,"Compliance and Control," Proc. 1976 Joint
Automatic Control Conf., San Francisco, 1976, pp. 694-699.

[30] R.Abraham, J.E. Marsden and T.Ratiu, "Manifolds, Tensor Analysis, and Applica-
tions," Addison-Wesley I 983.

82

[31] V.Guillemin and A.Pollack, "Differential Topology," Prentice-Hall, Inc., 1974.

[32] V.I.Arnold, "Mathematical Methods of Classical Mechanics," Springer-Verlag,
1978.

[33] IE. Marsden, "Lectures on Geometric Methods in Mathematical Physics," CBMS
Conference Series SIAM Vol. 37 I 981.

[34] D.T.Greenwood, "Classical Dynamics," Prentice Hall 1977.

[35] D.A. Wells, "Lagrangian Dynarrrics," McGraw-Hill Book Company 1967.