COOPERATION AND COORDINATION OF COMPUTER
CONTROLLED MANIPULATORS
By
SUJEET CHAND
A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY
UNIVERSITY OF FLORIDA
1984
Dedicated to
the memory of my father
ACKNOWLEDGEMENTS
In my five years at the University of Florida, no one person has had a more profound influence on me than Dr. Keith Doty, my principal advisor and the chairman of my supervisory committee. I gratefully acknowledge the numerous hours of his time in extremely stimulating discussions that surpassed the common barriers of working and leisure times. His constant stress on perfection and a lucid writing style is, I hope, reflected in this dissertation.
I am extremely thankful to Dr. Basile, Dr. Bullock, Dr. Dankel, Dr. Duffy and students at CIMAR for helpful discussions that contributed to this dissertation. I thank Dr. Ahmad Sadre and Bill Cartwright of General Electric Co. for the most pleasurable interface with the workings of the real world.
To my many friends and colleagues in Gainesville, I owe a special thank you for all the good times. Of course, this work would not exist today if it were not for the constant support of my parents and sisters, to whom I am forever grateful.
I acknowledge the financial support of the Department of Electrical Engineering during the years 198182 and 198283, and General Electric Co. for support as a Ph.D. Intern during the year 198384.
TABLE OF CONTENTS
PAGE
ACKNOWLEDGEMENTS .................................................... iii
LIST OF TABLES ...................................................... vi
LIST OF FIGURES ..................................................... vii
ABSTRACT ............................................................ ix
CHAPTER
INTRODUCTION ................. .............................. 1
II ONLINE POLYNOMIAL TRAJECTORIES FOR ROBOT MANIPULATORS ...... 9
Constrained Joint Motion  An Improved Technique ........... 10
Constrained Motion of EndEffector Polynomial Approximation of Joint Trajectories .............. 20
Effect of Error in Estimation of an Intermediate Velocity
on Preceding Velocities in a Cubic Splines Trajectory ....... 30
Application to Cubic Splines with
Equidistant Knots ...................................... 38
OnLine Cubic Splines for Joint Trajectory Interpolation .... 42
Scheme 1 ............................................... 42
Scheme 2 ............................................... 46
Acceleration Jump in OnLine Cubic Splines .................. 55
Results of Simulation and Experimentation ................... 60
III ROTATIONAL SPACE CURVES FOR CONSTRUCTION AND COMPLETE
DESCRIPTION OF ROBOT TOOL ORIENTATION TRAJECTORIES .......... 64
End Effector Motion Specification and Trajectory Planning... 65 Definition of A Rotation Space .............................. 69
Specification of Constraints on a Rotational Trajectory ..... 70
Explicit Constraints on the Rotational Trajectory ...... 71 Implicit Constraints on the Rotational Trajectory ...... 72
Construction of Rotational Trajectories Conventional Translation Dominant Approach .................. 74
Construction of Independent Rotational Motions .............. 83
Planning Constrained Rotation Transitions "Estimation of Acceleration for Spherical Wrist
Manipulators ........................................ 84
Decoupling rotational motion at the wrist joint... 85 Rotational Acceleration of the Tool Vector in
Rotation Space ......................................... 90
PAGE
Point to Point Unconstrained Transitions ....... t ............ 101
Independent Rotations  An Illustrative Example ............ 104
A Note on Wrist Rotational Velocity in Base Frame ........... 114
IV TRAJECTORY SPECIFICATION AND LOAD DISTRIBUTION FOR CLOSEDLOOP MULTIMANIPULATOR SYSTEMS ....................... 116
Planning a MultiManipulator Task ........................... 119
Task and Constraint Description ........................ 121
Trajectory Specification for a MultiManipulator Task ....................................... 122
Forces and Torques on an Effector in a Closed
MultiManipulator Chain ..................................... 123
Classification of Forces on an Effector
in a ClosedLoop ....................................... 124
Derivation of the Equations for the Force
and Moment at the Object Center of Mass ................ 124
Factors in a Force Distribution Scheme ................. 128
Definition of "Force" Frames ................................ 132
Derivation of Force Frames from the
End Effector Trajectories .............................. 133
Algorithms for ForceMoment Distribution .................... 138
Definition of Average Incremental Work
per Unit Force and Moment .............................. 139
A Decoupled ForceMoment Balance ............................ 141
Distribution of Moments ................................ 147
A General ForceMoment Distribution Scheme ................. 149
Force and Mement Equality Equations ............. 150
Inequality Constraints on Normal Forces
and Joint Torques ...................................... 151
Formulation of the Objective Function .................. 152
An Illustrative Example ........... ......................... 153
V CONCLUSION ......................... ............ ........... 163
REFERENCES .. ................... .... ............................. 166
BIOGRAPHICAL SKETCH ................................................. 170
LIST OF TABLES
PAGE
TABLE
2.1 Start point, stop point, and move velocity ................... 21
2.2 Acceleration factors ......................................... 22
2.3 Maximum joint velocities and maximum joint accelerations
for the robot of Figure 2.2 .................................. 23
2.4 Memory Error Factor for cubic splines with equidistant
knots (n = 8, Avi = vi AVa) .................................. 41
3.1 Move parameters for Figure 3.2 ............................... 80
LIST OF FIGURES
FIGURE
2.1 A sample joint trajectory .................................... 13
2.2 Kinematic structure of robot used in simulation .............. 24
2.3 Linear interpolation of joint trajectory ..................... 27
2.4 Sample points on a joint trajectory .......................... 31
2.5 Comparison of offline and online cubic splines.
Velocity estimation by linear interpolation (Eqn. (2.49)) .... 47
2.6 Comparison of offline and online cubic splines.
Velocity estimation by 1 point overlap, 2 point lookahead... 52
2.7 Comparison of offline and online cubic splines.
Velocity estimation by 1 point overlap, 4 point lookahead... 53
2.8 Maximum path deviation vs. path velocity and update rates
for linear and cubic splines interpolation of a typical
Cartesian move ............................................... 61
2.9 Online cubic splines showing no overshoot for an almost
linear joint trajectory ...................................... 63
3.1 A segment to segment transition .............................. 75
3.2 Rotational error vs. flyby time .............................. 81
3.3 Decoupling end effector velocity at the wrist joint .......... 88
3.4 A simulation move ............................................ 105
3.5 Trajectory of joint 1 vs. time for a Cartesian move of
the end effector ............................................. 107
3.6 Trajectory of joint 2 vs. time for a Cartesian move of
the end effector ............................................108
3.7 Trajectory of joint 3 vs. time for a Cartesian move of
the end effector ............................................. 109
PAGE
3.8 Trajectory of joint 4 vs. time for a Cartesian move of
the end effector ............................................. 110
3.9 Trajectory of joint 5 vs. time for a Cartesian move of
the end effector ............................................. 111
3.10 Trajectory of joint 6 vs. time for a Cartesian move of
the end effector ............................................. 112
4.1 Translation and rotation of an object about the base frame...126 4.2 3link planar manipulator used for simulation ................ 130
4.3 Torque on joint 1 for the same Cartesian move but
different startup configuration ............................. 131
4.4 Two manipulator move for simulation .......................... 155
4.5 Torque of joint 1 vs. time for the simulation move of
Figure 4.4 ................................................... 157
4.6 Torque of joint 2 vs. time for the simulation move of
Figure 4.4 ................................................... 158
4.7 Torque of joint 3 vs time for the simulation move of
Figure 4.4 ................................................... 159
4.8 Comparison of the sum of magnitude of joint torques for
an equal load distribution and load distribution
employing AIW factors ........................................ 160
4.9 Plot of AIWx vs. time for the simulation move of Figure 4.4..162
viii
Abstract of Dissertation Presented to the Graduate School of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Doctor of Philosophy
COOPERATION AND COORDINATION OF COMPUTER CONTROLLED MANIPULATORS
By
Sujeet Chand
December 1984
Chairman: Dr. Keith L. Doty
Major Department: Electrical Engineering
This dissertation develops new techniques for robot motion planning and force coordination in multimanipulator tasks.
We develop a new model for joint interpolated motion. A constraint on the trajectories of the faster, nonpacing joints clips both velocities and accelerations of the nonpacing joints without affecting the total move time. Experimental and simulation tests on the joint motion model clearly show the improved performance in controlled and smooth startups and setdowns of the manipulator joints and the end effector.
This dissertation develops the first complete formulation for online tracking of an end effector trajectory by piecewise cubic spline joint trajectories. A factor called the system memory error is defined to estimate an ideal number of lookahead points on the end effector trajectory for a prescribed level of performance in the joint trajectories. Two methods are described for estimation of an additional constraint at the end points of each computational block of the online joint trajectory generator. Excellent results were obtained in
simulation and experimental tests of the online splines for accuracy in tracking and smoothness of motion for a large number of constrained end effector motions.
A new dimension to robot motion planning is added by the introduction, definition and development of independent rotational motion planning for constrained and unconstrained rotations of the robot end effector. We define a rotation space at the wrist joint of spherical
wrist manipulators and prove that the maximum configuratonfree tool rotational acceleration in the decoupled rotation space is the fullload acceleration of the slowest joint in rotation space. A simulation example with independent rotational planning illustrates the elegance of this approach for planning "feasible" and accurate rotational motions.
We develop two linear models for "quasistatic" load distribution in the coordination of a closedchain multimanipulator task. The models employ moving coordinate frames on the object as reference frames for load distribution. The moving frames, called the force frames, are defined from the trajectory space curves of the object and the end effectors. We show that the constraint specification of the load distribution problem is simplified by the computation of all constraints in terms of the force frames. The load distribution is set up as a linear programming problem with constraints on the maximum surface normal forces and the maximum joint torques. The objective function minimizes the system power and attempts a load distribution in the ratio of the individual manipulator payloads.
CHAPTER I
INTRODUCTION
The recent years have seen an explosive growth in factory automation. Increasing demands on productivity, work in hostile environments and mundane and repetitive tasks call for immediate application of robotic technology. As demands on the robot's performance increase, the level of sophistication of robotic technology must also increase. The articulated robot manipulator, a design motivated by the dexterous human arm, is commonly applied in many industries for tasks ranging from assembly and part sorting to welding and heavy material handling. The productivity in a task involving several subtasks can be considerably increased if multiple manipulators work cooperatively on the subtasks. It is easy to conceive of a large number of tasks that can be made faster and more efficient with coordinated movements of multiple robots working cooperatively. A few multirobot systems available commerically provide restricted interaction between the multiple robots in the system. Olivetti was the first to market an industrial robot system with multiarm option as a special feature [Bedina 1977]. The Olivetti SIGMA system can be configured with two or three arms, the only restriction being that the total number of controlled degrees of freedom (motors) cannot exceed eight. A limited amount of cooperation is allowed between the robots. Although the arms work in the same workspace, the subtasks for each arm are independent. Collisions are avoided by stopping one arm until the
2
other moves out of the way. Thus, the Olivetti system is meant for tasks with independent subtasks, since direct interaction between the multiple robots is not allowed. In Japan, the Japanese government research institute has developed a prototype for an industrial robot called "Melarm" [Nakano et al. 1974]. Melarm has two arms, each with 7 joints and a gripper. Both arms are equipped with force feedback on every joint and several tactile sensors. An experimental system for automatic assembly of vacuum cleaners using two 8 degrees of freedom robots has been developed by Hitachi Ltd. [Takeyasu 1977]. Each arm has 3 fingers and about 30 tactile sensors on its grippers. There is no doubt that industry is aware of the numerous applications of cooperating robots to perform tasks hitherto impossible (or inefficient) for a single robot working alone.
Research in robotics is distributed in diverse interdisciplinary areas such as sensory preception (vision, tactile and force) [Gleason and Agin 1979, Inoue 1974, Raibert and Tanner 1982, Stute and Erne 1979], manipulator design [Warnecke and Shraft 1979], motion planning [Paul 1979, LozanoPerez 1981, Whitney 1972], transformations [Duffy 1981, Paul 1981], dynamics and servo control [Bejczy 1974, Golla et al. 1981, Hollerbach 1979, Luh et al. 1980b] and locomotion [Giralt et al. 1979, McGhee and Orin 1976, Taguchi et al. 1976]. This dissertation attempts to provide the sophistication in the techniques for robot motion planning and force coordination needed to provide multiple robots the capability to perform tasks requiring a high degree of interaction between the robots. Most of the techniques developed in this dissertation are directly applicable to a single robot controller for better path tracking in end effector controlled motions, smoother start
up and setdown in joint interpolated motion and planning independent rotational motions to make a rotation dominant trajectory "feasible" with smaller rotational errors.
In Chapter II, we reformulate the trajectory generation problem,
both for joint interpolated motion and end effector controlled motion. The reformulation is either an enhancement of the existing techniques or
a new solution to the problem. At the joint level, we formulate the joint interpolated motion by a model that allows the startup and setdown accelerations of each joint to be controlled. We introduce a new constraint on the motion of the nonpacing joints that clips the startup and setdown acceleration as well as the velocity of the nonpacing joints, without affecting the total move time. The performance of the proposed model was tested on a 6degrees of freedom industrial robot. The results clearly show the superiority of the proposed model to the existing model [Anderson and Paul 1979, Paul 1981, Whitney 1972].
A new solution is derived for the online joint trajectory approximation [Paul 1979] during a constrained end effector motion in Cartesian space. The online joint trajectories are constructed from the current point and a few lookahead points on the end effector trajectory. Most robot controllers employ a 2point lookahead on the end effector trajectory in order to linearly interpolate the joint coordinates between the current point and the two lookahead points, and provide a quadratic smoothing from one segment to another if a discontinuity in velocity is detected between the two linear segments.
A linearquadratic interpolation of the joint trajectory with a 2point lookahead [Paul 1979] produces considerable tracking error with higher
velocities and lower sample rates on the end effector trajectory. In
order to produce a better approximation of the joint trajectories with a limited point lookahead, we develop a scheme for online approximation of the joint trajectories by piecewise cubic splines. We formalize the estimation of the ideal number of lookahead points on the end effector trajectory in order to produce an accurate approximation of the joint trajectories by piecewise cubic splines. We define a factor called the system memory error, as a measure of the coupling errors in the estimation of the joint velocities in successive computational blocks on
the online cubic splines trajectory. The ideal number of lookahead points is defined such that the coupling in the determination of the boudary conditions of successive computational blocks is lower than a specified threshold. The derivation shows that for a piecewise cubic splines approximation of the joint trajectories with equidistant knots, 4 lookahead points in each block produces a system memory error of less than 0.5 percent. This result indicates that for an equidistant cubic splines approximation, a reasonably good trajectory approximation with
minimal coupling between successive computational blocks (less than 0.5 percent) can be obtained with 4 lookahead points on the end effector trajectory.
A derivation for the acceleration jumps on the online cubic splines for the joint trajectory approximation shows that the acceleration jump, to a first approximation, reduces, when successive computational blocks are overlapped for improving the velocity estimate at the block end points. The online cubic splines improve the tracking accuracy of the end effector by providing a better approximation of the joint trajectories. The improved tracking is illustrated by
5
sumulation. The online cubic splines were implemented on an industrial robot controller and tested for a large number of constrained end effector moves with different velocities and sample update rates. The online spline approximation technique showed excellent results in terms of smooth motion and tracking accuracy, for sample rates as high as 200ms and end effector velocities of up to 1.5 m/sec.
The current techniques for end effector motion planning for a single robot are "translation dominant" [Finkel 1976, Paul 1979, Taylor 1979]. In other words, the planning of an end effector motion is based on the constraints on the end effector linear velocity and acceleration. Thus, for a given move, the constraints on the tool translational motion are employed to determine the move time. This move time is also forced on the rotational trajectory. Independent rotational trajectories have been underplayed in robot motion since rotations are difficult to visualize, and the determination of the maximum constraints on the tool rotational motion is also difficult. Therefore, as long as the rotational tool motion is uniform, no constraints are imposed on the rotational trajectory. Since translation and rotation are independent motions, rotational planning should be decoupled from the translational move planning. In Chapter III, we introduce and develop the concept of independent rotation planning in the robot "rotation space." It is shown that for a translationdominant planning of a common move where the robot end effector transits from one linear move segment to another, the rotational error, as defined in Chapter III, is directly proportional to the translation move time, an unjustified dependence that is not necessary. Also, for several robot moves, the translation dominant approach may not yield a rotational trajectory that conforms to
the maximum constraints of the manipulator joint velocities and accelerations.
We employ a common geometry of industrial robots, namely, manipulators with spherical wrists, to derive a simple method for independent rotational planning. For a spherical wrist manipulator, we decouple translations and rotations at the wrist joint. The first 3 degrees of freedom, from the base to the wrist, are called the positioning degrees of freedom. The last 3 degrees of freedom are said to constitute the rotation space of the spherical wrist manipulator. This definition of rotation space simplifies the independent planning of the tool rotations. Since the manipulator is decoupled at the wrist joint, and we independently determine the rotation degrees of freedom and the translation degrees of freedom, the reverse solution for the manipulator is simplified, since we separately determine the positional and rotational joints. Thus, only a pseudo reverse solution is required to determine the positioning degrees of freedom when the rotational trajectory is preplanned in rotation space. The concept of a pseudoreverse solution with preplanned rotational motion is extremely powerful for producing fast and accurate end effector translational trajectories for the class of manipulators with nonspheri.cal or offset wrists, that only permit a recursive reverse transformation.
In order to plan rotational motions independently, we require an estimate of the maximum rotational acceleration of the tool at any point in the robot workspace. The decoupling of the translation and rotation ensures a less conservative estimate of the rotational acceleration as compared to the case where the decoupling is not possible, since the rotational velocity and the acceleration constraints are determined
7
independently for the first 3 and the last 3 degrees of freedom. The last 3 joint angles are usually twice a fast as the first 3 joint angles and therefore yield a higher estimate of the tool rotational velocity and acceleration. We derive the maximium limit on a value of the tool rotational acceleration that can be employed any where in the usable robot workspace (except at the singularity and limit points) and show that this acceleration is the fullload acceleration of the slowest joint in the decoupled rotation space. We also describe an algorithm for unconstrained point to point rotational transitions employing the maximum limitations on the joint rates and accelerations for a "feasible" rotational trajectory. A simulation of independent
rotational motion planning clearly illustrates the elegance of the approach for producing smooth and feasible rotational motions for both constrained and unconstrained tool rotations.
In Chapters II and III, we developed new solutions for joint interpolated motion, accurate online approximation of joint space trajectories and the construction of independent rotational motions of the robot end effector. Coupled with the existing techniques for translational motion planning, we now have the capability to generate accurate and feasible joint trajectories and end effector motions for a given robot move. Let us consider the task of displacing a long or heavy object by one or more manipulators. Given the destination of the object and the desired trajectory of the object, we can generate the end effector and joint trajectories of each robot. In order to determine the torque for each joint motor for a given trajectory, we require an active distribution of the object load among the manipulator end effectors holding the object at each sample point on the trajectory. Dynamic load distribution in a closedloop multimanipulator task is
underspecified [Orin and Oh 1981, Williams and Seireg 1979]. Since theoretically infinite solutions for the load distribution are possible, in Chapter IV, we develop two linear models with an objective function for a load distribution that minimizes the overall system power.
We formulate the load distribution in Chapter IV in moving coordinate frames at each end effector. A moving coordinate frame called the force frame is defined from the space curve of each end effector trajectory. The 3 axes of the force frame are the tangent, normal , and binormal of the end effector trajectory space curve. The underspecified load distribution is set us as a linear programming problem, with an objective function for minimization of the system power for unit forces and moments in each direction of the force frame axes. By eliminating a direct dependence of the objective function on individual joint constraints, the objective function can be initially evaluated as any nonlinear function of the joint parameters for unit forces and moments in the directions of the force frame axes. This nonlinearity does not affect the linearity of our model, since the objective function is made independent of the individual joint parameters. In order to verify the load distribution algorithm, we simulate a simple two manipulator closedloop task. The simulation illustrates that the load distribution algorithm of Chapter IV reduces the overall joint torques of the manipulators in a closedloop task. An "equal" load distribution is employed to provide a comparison of the overall joint torques for the load distribution scheme of Chapter IV.
Chapter V summarizes the main ideas in this dissertation, and provides insight into future work in the cooperation and coordination of multi robot operation.
CHAPTER II
ONLINE POLYNOMIAL TRAJECTORIES FOR ROBOT MANIPULATORS
A smooth, accurate and predictable trajectory of all robot joints is of utmost importance in a multirobot task. Smooth trajectories for the robot joints and consequently, the end effector, are a necessity for
cooperating robots. Robot motions can be classified into two types. One type of motion is called the joint interpolated motion. The other
type is controlled motions of the robot end effector. In a joint interpolated move, the robot is moved from one set of joint coordinates to another without any control over the end effector trajectory. We develop a model that employs a splined polynomial trajectory for the joint trajectories in a joint interpolated move and describe a technique for scaling both the velocity and acceleration of the faster joints in a joint to joint move.
Traditionally, there are two approaches to planning the joint trajectories for controlled end effector motions. One is the offline
approach where all the joint trajectories are planned prior to the move. The other approach is to determine the joint trajectories online
at a certain sample rate during the actual motion. The planning of offline trajectories is generally not a time critical process. Therefore, several wellkiown techniques for polynomial trajectory approximation are directly applicable, the complexity and the accuracy of approximation is determined by the task requirements. The online trajectory problem, however, has received little attention since the
lack of sufficient number of constraints on the online trajectory and the time constraint for computation limits the degree of accuracy of polynomial approximation. In this chapter, we develop the constraints on the online trajectory in order to produce a cubic splined trajectory for each joint. We derive the ideal number of lookahead points for generating an online joint trajectory, based on an approximation error factor called the system memory error. An analysis of the acceleration jumps in the online cubic spline trajectories is also given. The application of these trajectories to robot motion has been studied both by simulation and actual experimentation on an industrial robot. Both studies indicate excellent results in terms of smoothness of motion and accuracy of tracking the actual trajectory.
Coordinated Joint Motion  An Improved Technique
This section describes joint interpolated motion employing a splined polynomial trajectory. The most frequent use of a joint interpolated move in an industrial environment is for fast transits of the robot arm between jobs. A joint interpolated move provides the fastest transition of the robot arm in its workspace. The other type of move, namely a controlled motion of the end effector, is slower due to the conservative upper limit on the end effector speed (a value that is determined from the joint parameters and is valid for the entire robot workspace). Therefore, a joint interpolated motion is employed for fast but unconstrained transitions o the robot end effector between points.
A well known technique for joint interpolated motion is to determine the slowest joint for the move followed by scaling the velocities of the remaining joints such that the move time of each joint
is that of the slowest joint [Anderson and Paul 1979]. All joints are initially accelerated to their maximum accelerations until the desired joint velocity is achieved. The joints then cruise at the desired velocities until the end of the move, where the maximum deceleration is
applied on each joint. Due to the fast initial startup of all the joints at their maximum accelerations, the end effector (whose trajectory is not a concern in joint interpolated motion) experiences a
large discontinuity in motion at the start (and end) of the motion. If the effector has a delicate tool fixture, the user may be prompted to use tool controlled motion (and sacrifice speed) for intermediate transitions. In order to reduce the initial discontinuity in end
effector motion, we model a joint trajectory such that we start each joint motion by a cubic polynomial. The slope of the acceleration can
be defined by the user as the slowness for startup. After a slow startup, the end effector motion is smooth, since all the joint
trajectories are continuous. We define a general model for joint interpolated motions employing a cubic startup with a specified degree of "slowness." If the speed of the transition is of higher concern than
the smoothness in startup and setdown, the cubic polynomial can be simplified to a quadratic.
The new idea presented in this section is to coordinate the motion of all joints during startup, constant velocity and setdown segments. Thus, instead of coordinating the joint motion between start and stop points, the joints are required to be on the startup segment, constant velocity or the setdown segment for the same periods of time, determined by a pacing joint. It is shown that by assuming the same startup time for all the joints, initial jerk on some of the nonpacing
joints is reduced, making the startup (and setdown) motion smoother. If the joint motion has to pass through an intermediate point then we could either use a cubic spline to pass through the point, or a
quadratic polynomial as in [Paul 1979, Taylor 1979] for bypassing the point. A cubic spline is likely to produce an overshoot at the
intermediate point, which may be unacceptable if the joint is close to a limit at the intermediate value. Therefore, a quadratic flyby is preferable if overshoot is not desired. Once again, all joints are on the flyby path for the same period of time determined by a pacing joint for flyby. By forcing all joints to be on the flyby path for the same period of time, acceleration requirement of the nonpacing joints is reduced. We now develop a model for the joint trajectories for a pointtopoint joint interpolated motion.
In Figure 2.1, let J1 and J2 be the start and stop points for a pointtopoint move of a joint. The joint trajectory is divided into 3 segments called the startup, cruise, and setdown segments. A cubic polynomial is employed for the startup and setdown segments in order to vary the speed of startup and setdown. Assuming the joint starts
from rest, the joint acceleration increases linearly from 0 to the required acceleration for startup. The acceleration profile of the joint is made up of 3 segments. For an initial period tacc, the joint accelerates linearly to the maximum servo acceleration. The maximum acceleration remains constant for a period tconst and then drops linearly to zero in another tacc seconds, such that the desired velocity of the joint is achieved. The joint acceleration for the startup time ta can be modeled as follows.
5"(rad/sec2 )
t t c a
C " c a
1 tacc t c ta
Figure 2.1 A sample joint trajectory
t
acc
6
(rad/sec)
9
max
t
(Secs)
(secs)
mt o
  acc
a(t) = amax tacc (tacc +tconst)
amax  m (t  tacc  tconst) (tacc + tconst) < t < ta (2.1)
where amax is the maximum servo acceleration (known from the servo
characteristics) and m is a user defined slope. The "slowness" of startup and setdown is determined by m. The polynomial for the joint trajectory will be a cubic for 0 < t < tacc and (tacc + tconst) < t < ta , and quadratic for tacc t t a if tacc :0, then the startup and setdown polynomials will be quadratics. Also, if ta = 2tacc, the polynomials will be cubic. The condition to check, namely ta acc , is equivalent to vd > a max/m , where vd is the desired velocity of the joint.
In order to produce the fastest transition during the cruise
segment, we employ a polynomial of degree one for the joint trajectory between the startup and the setdown segments. The motions of all joints are coordinated by the determination of the joint that takes the most time for the pointtopoint move. This joint is termed the pacing joint. The pacing joint time scales the move parameters of the nonpacing joints.
Integrating Eqn. (2.1),
S0o+ 0.5 mt o < t < tacc
v(t) = V(tacc) + amax(t  tacc) tacc < t < (tacc + tconst)
I V(tacc + tconst) + a max(t  tacc
 t const) 
0.5 m (t  tacc  tconst)2
(t acc + t const) < t
(2.2)
where v. is the initial or start velocity, v(tacc) is the velocity after tacc seconds, V(tacc + tconst) is the velocity after (tacc + tconst) seconds. Integrating Eqn. (2.2) to get the equation for the joint displacement, and assuming vo = 0 (joint starts from rest),
J(t) =
J(0) + mt 3/6
O
  acc
J(tacc) + V(tacc) (t  tacc) + 0.5 amax (t  t ac)2
tacc < t < (tacc
+ tconst)
J(tacc + tconst) + {v(tacc + tconst)I (t  tacc  tconst) +
amax (t  t acc  tconst) 2/2  m (t t acc t const)3 /6 (tacc tconst)
(2.3)
Let vd denote the specified velocity of a joint for the pointtopoint move. This velocity is usually given as a percentage of the
maximum joint velocity. T determined from Eqn. (2.2) as
d/amax max/m
ta
( 2vd/m) /2
he startup and setdown time ta can be
t > 2t
a
ta = 2tacc
(2.4)
Let tab denote the time for the cruise segment. If A6 is the joint displacement between the 2 points of the pointtopoint move, then
Ae (A6)2t a+ vd . tab
a
tab = (l/Vd) {AO  (A)2tl}
(2.5) (2.6)
where (Ae)2t denotes the joint displacement in the startup and seta
down segments. We can derive (A)2ta from Eqn. (2.3) and Figure 2.1 as
3
(e)2t = 2 [mt 3 /2 + v(ta ) . t +
acc acc const+
0.5 amax t2onst + V(t acc+ tconst tacc
(2.7)
In Eqn. (2.7),
tacc = amax/M
(2.8)
tconst = (ta 2 tac)
(2.9)
V(tacc) = 0.5 amax tacc (2.10)
V(tconst + tacc) = 0.5 amax tacc + amax tconst (2.11)
Thus, from Eqns. (2.4) and (2.6), the move time for each joint can be determined as (2ta + tab). The joint that gives the maximum move time is the pacing joint.
Let Ti denote the move time for the pacing joint and the pointtopoint move. In order to coordinate the motion of all the joints, the move parameters for the nonpacing joints must be determined based on the total move time, Ti. An accepted and the most frequently used method for determination of the move parameters of the nonpacing joints is to determine the velocities of the nonpacing joints, assuming that each joint accelerates to its maximum acceleration (ta > 2Tacc) during startup (and setdown) [Paul 1979]. From Eqn. (2.4), assuming ta > 2tacc (the equations for ta = 2Tacc can be derived similarly),
vd = amax (t a max/m) (2.12)
Since amax, m are known, we need to determine ta for each nonpacing joint. Expressing (Ae)2t of Eqn. (2.7) in terms of m, ta and
a
amax, and substituting in Eqn. (2.5), we get
(Ae)npj = (amax) t2 + (a2ax/m + a Ti) t  a2 T./m (2.13)
a ) t aax/ a ax max 0 "a max i
(ama) t 2 + (a 2x/m + a T) t  a2 T./m (Ae) =0 (2.14) mx a mx max i a max i npj
From Eqn. (2.14), we can solve for ta, We then evaluate vd from Eqn. (2.12). Thus, the move parameters for the nonpacing joints can be determined from m, amax' Ti, and AB.
The determination of ta by Eqn. (2.14) and vd by Eqn. (2.12) clips the velocities (from the user specified percentage) of the nonpacing joints, without changing m and amax. Since some of the nonpacing joints may have smaller displacements and are faster than the pacing joint, the startup and setdown of these joints can be made slower than the pacing joint. The slower startup of some of the nonpacing joints smooths the initial jump in the end effector. This may be significant for a delicate tool attachment at the end effector. We now develop a simple technique for the determination of a factor for the slower startup of the nonpacing joints. This factor is a smaller value of "m" for each nonpacing joint.
Since we now introduce an additional variable "m" in the
determination of the nonpacing joint parameters, we need an extra equation or constraints for a unique solution. We realize the extra information by imposing the constraint that the nonpacing joints accelerate, cruise and decelerate to a stop for the same periods of time
as the pacing joint. In other words, we coordinate the motion of the joints during the startup segment, cruise segment and the setdown segment. This constraint improves the esthetics of the motion, and as shown later, reduces the initial jerk in the end effector. We note that for a given move, there may be one or more nonpacing joints that will not yield the desired displacement with the above constraint. We now derive the condition for coordinating the motion of a nonpacing joint during startup, cruise, and setdown segments of the pacing joint. Let
m' denote the new startup slope for a nonpacing joint. From Eqn. (2.13), substituting m' for m, we can derive an equation for m' in terms of ta, ti, amax, and (Ae) npj as
{a2max (ta  Ti)} / {(Ae)npj  amax ta (Ti  ta)} (2.15)
If m' is less than m, then the nonpacing joint motion can be coordinated with the motion of the pacing joint during the 3 segments with a slower startup and setdown. If m' is greater than m, then the nonpacing joint cannot be coordinated during startup, cruise, and setdown of the pacing joint. In this case, the move parameters for the nonpacing joint are determined from Eqns. (2.12) and (2.14). This case can occur when the maximum acceleration of the nonpacing servo is less than the maximum acceleration of the pacing servo, and the displacements of the two joints are comparable. Thus, by employing Eqn. (2.15), we can determine the move parameters of the nonpacing joints, which are considerably simplified when m' < m, since ta and tab are known from the pacing joint parameters.
In the above paragraphs, we developed a model for pointtopoint joint moves with cubic polynomials for startup and setdown. For manipulators with heavily damped servos, tacc may be specified as 0 which makes the startup and setdown trajectories quadratic polynomials. For a quadratic startup, by imposing the constraint that
the startup time of a nonpacing joint is the same as that of the pacing joint, we can reduce the initial jump in the acceleration of the nonpacing joints. If k.amax denotes the acceleration of a nonpacing joint, then k can be derived as
k =(AO) npj / {amax ta (ta + tab)} (2.16)
Note that k = 1 for the pacing joint. If k < 1 for a nonpacing joint, then startup, cruise, and setdown times of the pacing joint can be employed for the nonpacing joint, with the nonpacing joint acceleration clipped to k.amax, k < 1. If k > 1 for a nonpacing joint, then the joint trajectory is planned such that the startup and setdown times are determined independently of the pacing joint, and only the cruise velocity is clipped such that the move time is Ti.
In order to illustrate the acceleration clipping factor "k" of Eqn. (2.16), Table 2.1b shows the value of k for each joint for a number of pointtopoint joint moves of Table 2.1a. The manipulator of Figure 2.2 is used for simulation. The maximum joint velocities and the maximum joint accelerations for the manipulator of Figure 2.2 are shown in Table 2.2. Note that for all the moves, the acceleration can be clipped for most or all of the nonpacing joints. The clipping can be greater than 9OT of the maximum joint acceleration for some nonpacing joints.
In the following sections, we describe the online determination of
joint trajectories for controlled motions of the robot end effector Cartesian space.
Constrained Motion of the End Effector Polynomial Approximation of Joint Trajectories
In a constrained end effector motion, the trajectory of each joint must be controlled to result in the desired trajectory of the end effector. The use of polynomial functions for generating accurate and smooth joint trajectories is a well known technique [Finkel 1976, Lewis 1974, Luh et al. 1983, Paul 1972]. It has been shown that a large number of
21
Table 2.1 Start Point, Stop Point, and Move Velocity
START POSITION (DEG.) DESTINATION (DEG.)
MOVE
1
2
3
4
5
6
7
8
9
START POSITION (DEG.) (el,e2,' 3,'4,05,'6) 0,0,0,0,0,0 10,25,30,45,60,75 10,10,10,10,10,10
10,10,10,10,10,10 10,20,30,45,45,45 10,20,30,45,45,45 10,20,30,45,45,45
60,30,40 ,45,10,50 50, 10 ,45,60,30,40
10 45,45,45,60,60,60 30,50,60,30,30,30 70
DESTINATION (DEG.)
(ele2, o3,e4,e5,e6) 30,45,45,20,50,40 80,45,45,45,45,45 60,60,60,60,60,60 60,70,100,60,60,60
20 ,20 ,60 ,40 ,40 ,40
100 ,45 ,45,70,70,70
10,30,40 ,45 ,45,45
10,50 ,20 ,10,50,70 70,50,10,20,50,10
VELOCITY
70
50 70 40
30 100
60 80 60
22
TABLE 2.2 Acceleration Factors
MOVE MOVE TIME (SECS.)
# tacc tconst Ti kI k2 k3 k4 k5 k6
1 0.183 2.321 2.686 0.716 1 0.882 0.199 0.464 0.375 2 0.165 4.456 4.787 1 0.266 0.176 0 0.083 0.168
4
.183
0.104 0.125 0.33 0.185 0.264 0.156 0.183
9
2.599 5.737
6.335
3.3
1.905 2.624 2.439 5.103
2.965 5.946 6.585 3.962 2.275 3.153 2.753 5.468
1.073 0.894 0.406
1 0 1
0.537 0.847
1 1
0.504
0.55
0.263 0.266
1 1
0.882 1.121
1
0.56
0.232 0.704 0.772 0.975
0.448 0.373 0.48 0.094 1.062 0.208 0.897 0.425
0.418 0.348 0.447 0.088
0.99
0.222 0.209 0.396
0.422 0.351
0.452 0.089
1
0.112 0.527
0.4
Table 2.3
Maximum Joint Velocities and Maximum Joint Accelerations for the Robot of Figure 2.2
MAXIMUM MAXIMUM JOINT ANGLE JOINT VELOCITY (rad/sec) JOINT ACCL. (rad/sec2)
01 0.515 1.5575 62 0.4365 1.6725
63 0.7595 1.896 64 1.257 3.728
05 1.24 4.00 e6 1.22 3.961
z
Figure 2.2
24
56
05
Kinematic structure of robot used in simulation
undesirable properties of high order polynomials can be overcome by using lower order spline functions [Finkel 1976]. If a trajectory has a large number of points, then a spline fit for the trajectory requires all spline coefficients to be computed before the trajectory is executed. This is due to the inherent nature of spline calculations [Ahlberg et al. 1967, DeBoor 1978]. Hence, a spline trajectory with a large number of intermediate points has to be computed offline. Techniques for piecewise splines using Xsplines [Clenshaw and Negus 1978] have also appeared in literature [Lin and Chang 1983]. In the following sections, we investigate the ideal number of lookahead points for a piecewise cubic spline trajectory and describe two novel techniques for breaking the offline spline computations into smaller blocks for realtime trajectory calculations.
An end effector move is executed by transforming the end effector position and orientation into joint coordinates at a large number of points on the end effector trajectory. If the move is to be in real time with online trajectory calculation, we lookahead a few points on
the end effector trajectory and generate the joint trajectories. The complexity of the interpolation scheme used to approximate the joint trajectories directly depends on the time interval between successive joint coordinates, i.e., the sampling rate on the end effector trajectory. If the sampling rate is high, then a polynomial of degree 1 may be adequate. For slower sampling rates, linear interpolation of joint coordinates will produce jerky motion and may even cause instability [Paul 1981]. In order to demonstate a typical joint trajectory during a constrained end effector motion, a plot of joint displacement versus time for a joint angle of a 6degrees of freedom
robot for one particular move is shown in Figure 2.3. Joint target points for the elbow rotation (angle e4) were generated at equal time intervals for a Cartesian move of the end effector of a robot whose kinematic structure is shown in Figure 2.2. A polynomial of degree one (straight line) is used for interpolation between successive target points. This form of interpolation can be unsatisfactory for a smooth servo response because a linear function in displacement produces impulsive accelerations between constant velocity segments.
A well known technique for smoothing a joint trajectory is to use polynomials of a degree higher than 1. A single polynomial fit through all the intermediate points of a joint trajectory results in an extremely high degree for the polynomial. For instance, if the joint trajectory has n intermediate points, the minimum degree of one polynomial that passes through all the points is nI (Lagrange interpolation). Splining low order polynomials provides an elegant way of reducing the overall degree and also the computational burden of trajectory generation. The degree of the polynomial should be such that the overshoot and wandering are minimal [Finkel 1976, Lewis 1974]. Overshoot occurs when the value of the polynomial goes beyond the "band" of initial and final values. Overshoot becomes obvious for a quartic polynomial when the "band" between start and end point is small. Wandering is retrograde motion. Both overshoot and wandering are undesi rable.
A number of papers in recent literature have described the application of spline functions to the generation of smooth robot joint trajectories [Luh et al. 1983]. Splined polynomials are used for pointtopoint joint motion and motion involving a large number of
IIETA (DEGREES)
27. 1. 23.0t 2fl 0 17.0 14.5
11.3 0.21
5. 611 1.91
1.23
4.3o
7.54
IM O
13.0
2,1. 1
lot. 211. 241. 271.
TIM1E (MG)
Linear interpolation of joint trajectory
3M 3 2 Oil. 4 05.O 1201.
Irk
1
Figure 2.3
I
!Y
28
intermediate joint positions. [Luh et al. 19831 describes the construction of cubic spline joint trajectories with optimum placement of the knots. The cubic splines are constructed offline with a knowledge of the end effector trajectory. A classification of schemes using splined polynomials for constructing a joint trajectory with a large number of knots is as follows:
(1) 4334 trajectory
The first segment is a 4th order polynomial specifying the
trajectory from initial to the "liftoff" position of the joint [Paul 1981]. The following polynomials are all cubics except for the last trajectory segment which is a quartic.
Consider the joint displacement trajectory shown in Figure 2.3. At the initial and destination points, the position, velocity and acceleration are known. At each intermediate point, the position or joint displacement is known. To make the interpolation curve doubly continuous at all points, the constraint that acceleration be continuous at the intermediate points where 2 polynomials join is imposed. Using this constraint at the intermediate points, a set of equations are generated that determine the slope of the curve at the intermediate points where 2 polynomials are splined together [Ahlberg et al. 1967, DeBoor 1978]. A formulation of this type results in a 4334 trajectory.
(2) 333 trajectory
This is a cubic spline fit for all the trajectory segments. The only difference from the 4334 scheme is that the acceleration constraints at the start and destination points are ignored to yield all cubic polynomials.
Inherent in the spline formulation is determination of the slopes of the interpolating polynomial at all intermediate points between start point and end point. Thus, if there are n intermediate points, it is necessary to solve a matrix equation of dimension nXn. The equation involves solution of a tradiagonal matrix, which is uniquely solvable by the LU decomposition scheme [Johnson 1977]. A typical motion trajectory of the end effector might result in 50 or more intermediate
points for a joint trajectory. Solution of a matrix equation of dimension 50x50 is impractical both in terms of computational time and storage requirements and is feasible only for an offline trajectory planning scheme. For many robot applications, an offline generated trajectory is unacceptable. A specific example is sensor based control of the robot end effector, where the trajectory cannot be preplanned. In this case, splines for the joint trajectories must be generated online. Two schemes employing cubic splines for approximating joint trajectories with limited point lookahead are derived in this chapter. Experimentation has shown that (1) the online cubic splines provide smooth motion of the joints (and end effector) for a wide range of sampling rates, and (2) the computations that determine the required spline coefficients are simple enough for real time implementation. Also, simulation has shown that for slower sampling rates, the splines track the end effector trajectory much better than a simple linear interpolation.
The following sections of the chapter are organized as follows. The next section derives an "ideal" number of lookahead points for generating cubic spline trajectories online. We then develop techniques for estimating an additional constraint at the last point of
a lookahead block. An analytical derivation of the acceleration jump in the online splines scheme is also given. Finally, the results of experimentation and simulation are described.
Effect of Error in Estimation of an Intermediate Velocity
on Preceding Velocities in a Cubic Splines Trajectory
In order to compute splines for a joint trajectory online, it is necessary to look ahead a few points on the end effector trajectory. The current point and the lookahead points are referred to as a
"block," the block size being determined by the number of points in the block. Ideally, the size of a block should be such that an error in estimation of an additional constraint at the last point of the block should be localized to the block itself. For most robot controllers, the block size is fixed by the computational bandwidth of the controller
for the determination of the lookahead points on the end effector trajectory. This section describes a method for the estimation of an ideal block size, which can be implemented if the controller provides the flexibility for changing the block size.
Figure 2.4 shows a segment of an example joint trajectory with the start point P1, and two intermediate points P2 and P3. The block size in Figure 2.4 is assumed to be 3 (2 point lookahead). Two cubic
polynomials with acceleration continuity at the intermediate point P2 can be evaluated for the present point P1 and the two lookahead points P2 and P3, if one additional constraint at P3 is known. At P1, the position and velocity are known, at P2 and P3 the only information available is the position (82 and e3). By splining 2 polynomials with continuity in acceleration at P2, the velocity at P2 can be determined [DeBoor 1981]. Therefore, with 4 constraints between P1 and P2, a cubic
ee
P2 PP4
Pl
2 1 Ti
Ti 3
tI t2 t3 t
Sample points on a joint trajectory
Figure 2.4
polynomial for the trajectory P1P2 can be computed. The polynomial P2P3, however, will be a quadratic since only one boundary condition (position) is known at the point P3. If an additional constraint at the
point P3 (velocity or slope of the curve) is estimated, then a cubic polynomial can be evaluated for the segment P2P3. This sections
studies the effect of the velocity at the block end point (P3 in the example of Fig. 2.4) on the velocities at the preceding points.
An all cubic splines (333) trajectory with n intermediate points yields a tridiagonal matrix equation for the determination of the
slopes vl,v2,...,vn at the n intermediate points [DeBoor 1978, Luh et al. 1983].
all a12 0 0 0 a21 a22 a23 0 0 0 a32 a33 a34 0
0 0 0 0 0 0 an,n1 an,n bI b2 b3
L bn  vn+(1 * Tn)
0
0
0
Kv(I) v(2) v(3)
v(n)
(2.17)
where the coefficients aij and bk are
ai'i = 2 * (Ti + Ti+l)
ai i_1 = Ti+l
aii+ I = Ti
bi = (3* Ti+i/Ti) * (0i  ei_l) +
(3 * Ti/Ti+l) * (i+1  oi (2.18)
with Ti, Ti+1 being the ith and the (i+l)th time intervals, ei the ith joint position at time ti and vo and Vn+I are the velocities at the start and destination points (see Ahlberg et al. 1967, Luh et al. 1983) for details on the derivation of Eqn. (2.17)). The matrix equation (Eqn.
(2.17)) is derived by imposing continuity in acceleration at the n intermediate points [Ahlberg et al. 1967, DeBoor 1978]. This equation can be uniquely solved for vl,v2,...,vn recursively [Johnson 1977]. We will show that if Eqn. (2.17) is broken into a number of smaller segments for piecewise splines, then an error in estimation of the velocity at the end of each smaller computational block affects the slopes at only a few preceding points, beyond which the error, as defined later by Eqn. (2.26), becomes negligible. It will be shown that
for a cubic splines trajectory with equidistant knots, an error in estimation of an intermediate velocity causes errors of significant magnitude (in decreasing order) only in the 4 preceding velocities.
35
A recursive solution to (2.17) can be derived as follows:
Yj = aj,j+i/oj
j = 1,2,...,nI
Yn = Tn/n
Also define
j = aj,j aj,j. Yj1 a, = all
zj = (bj  aj,j_l)/Oj zI = (bI  vo T1)/OI
then
vj zj  vj+i Yj
j = 2,...,n
(2.19)
j = 2,...,n
where aij denotes the element of the ith row and jth column of the nXn matrix on the left hand side of Eqn. (2.17) and bj denotes the jth element of the vector on the right hand side of Eqn. (2.17).
We will now derive the intermediate velocities vI ... vn in terms of the last velocity, vn1, and other terms involving zj and yj. The equations so derived will demonstrate the effect of a change or error in the velocity vn+I on the intermediate velocities.
Define
j = n,...,1
(2.20)
Vn = zn  Vn+1 (Tn/On) VnI = Zn1  Zn YnI + Vn+1 (Tn/On) Yn1
V1 = Z1  Y1 z2 + Y1 Y2 z3 
+ (1)n Vn+1 Yn YnI Yn2 "" Y1
(2.21)
Now, if vn+1 becomes Vn+1 + AVn+1, then the magnitude of the error in v, is given by AvI as follows:
AV = (AVn+1) Yn Yn1 ... Y1
= (AV n+1I) (T nl/0n) (T n 1/0 n 1) . (TII /a1)
(2.22)
we now show that the product of the terms on the right hand side of Eqn. (2.22) becomes 0 in the limit as "n" approaches infinity. This is easily demonstrated by showing that 0 < (Ti/si) < 1 for i = 1 to n.
1/T 1 = 2 (T1+ T2)/T
= 2(1 + T2/T1) (2.23)
Since T2 and T, are both positive, a1/TI is positive and greater than 1. From (2.23) and (2.19)
2/T 2 = 2 (T2 + T3)/T2  (T3/T2) (1/(s1/T1))
= 2(1 + (T3/T2) * (1  0.5/(01/T1))) (2.24)
From (2.23) and (2.24) 02/T2 is also greater than 1. By induction,
Oi/Ti = 2 (1 + (Ti+i /Ti) * (1  0.5/(aii/Ti_))) (2.25)
is also greater than 1. Therefore, the right hand side of Eqn. (2.22) progressively reduces in magnitude with increasing "n". From Eqn. (2.22) we define a memory error factor lj > 0 as a measure of the influence of a change in the slope at the (n+l)st knot on the slope at the jth knot (j < (n+l)),
n
P. = (Tj/aj)(Tj+/j+) ... (T n/n)= II Yk (2.26) k=j
hence AVj = pj (AVn l)
Since lim pj = 0, a practical threshold value of pj can be
j +W
defined for a given application such that for some finite j, the error
in estimating the velocity at the (n+l)st knot has no practical effect on the jth knot. In this chapter, we assume the system memory error is
negligible when 1I < c and c is less than 0.5%. Thus, given the sampling intervals, we can determine an "ideal" block size (n) such that the error due to estimation of the slope at the last point of the block is localized to the block itself namely il < e, where e > 0 provides some prescribed level of performance in trajectory motion. The next section applies these results to a cubic splines trajectory with equidistant knots.
Application to Cubic Splines with Equidistant Knots
If the knots of a joint trajectory are equidistant in time, then the elements of the matrix in Eqn. (2.17) simplify as follows:
a. 4T , a i l1i= T, a ii+l= T (2.27)
Substituting for aij in the expressions for yj, aj and zj in Eqn. (2.19) we obtain
j= T/ j =n .
Yn =T/On
j= 4T  T2/Oj.I
1= 4T
zj = (bj  Zj_l)/ j
zI = b1/4T
i = nI,...,2
(2.28)
Now sj can be computed from (2.28) as follows:
01 = 4T, 02 = 3.75T, $3 = 3.73343T,
04 = 3.73218T, 05 = 3.73218T,...,n_1 = 3.73218T
Note that On appears to converge to the value 3.73218T for n ) 4. We now show analytically that an always converges to the value (2 + V3)T for a cubic splines interpolation with equidistant knots.
Consider
On  n1
nT(40n1  T)/an1  0n1 S (ni  4Tn + /n= _ (BnI  1) (OnI  2)/On.1
(2.29)
= (2 + VI)T = (2  VI)T
(2.30)
(2.31)
where
From Eqn. (2.29), it is seen that the a sequence is monotone decreasing for an>181 or $nI < 82 . Since a1 = 81 (Eqns. (2.28) and (2.30)), the 8 sequence is monotone decreasing, and
8n 81 = (2 + V )T (2.32)
Since j = T/j,
lim
ji Y = T/(2 + VI)T = 2  V (2.33)
From Eqn. (2.26), it is seen that the effect of a change in the velocity of the last point (Avn+,) on the velocity of the i th point
(vi) is succintly described by pi = (Avi/Avn+l)' a value only dependent upon i 4 n. An example trajectory with 10 points is used to demonstrate
the influence of a change in the last velocity on the preceding velocities. Initially, it is assumed that the position and slope of the
trajectory at the start and final points are known. At the eight intermediate locations, only the position coordinate is known. Table 2.3 shows the changes in the velocities of the eight intermediate points due to a change Av9 in the last (tenth) velocity. The table shows that a change in the velocity of any point on the cubic spline trajectory results in significant changes only in the preceding 4 velocities, beyond which, the effect of the change becomes insignificant, less than
0.5% of the change.
TABLE 2.4 Memory error factor for cubic splines
with equidistant knots (n=8, Avi = Pi Av9)
i Ii
8 0.26794 7 0.07179 6 0.01923 5 0.00515 4 0.00138 3 0.00136 2 0.00009 1 0.00002
The numbers in Table 2.3 provide an important general result. They show that for an all cubic splines interpolation with equidistant knots, an error or change in the slope at a lookahead point induces relatively significant errors (pj > 0.5%) for only the preceding 4 points. The slopes can be considered unaffected for all earlier points. The memory of the interpolation is said to be 4. This result can also be interpreted the following way. If the offline splines calculations are broken into blocks of 6 points each, and we estimate the velocity at every sixth point, then the error in estimation of the velocity is limited to errors in its block of 4 intermediate points.
The next section describes schemes for estimating the slope of the trajectory at certain intermediate points for online spline computations with limited point lookahead.
OnLine Cubic Splines for Joint Trajectory Interpolation Scheme 1
One way of determining an additional constraint at the last point of a lookahead block is to compute the joint velocities by an "inverse velocity" transformation. There are several approaches to an inverse velocity determination. Two general approaches are the use of an inverse Jacobean matrix [Whitney 1969], and Screw theory [Lipkin and Duffy 19821. The data required for an inverse velocity transformation are end effector linear and rotational velocities and the configuration of the arm. The complexity of the inverse velocity transformation is dependent on the geometry of the manipulator. For many 6 degrees of freedom robots with intersecting axes at the wrist (spherical wrist), the inverse velocity computation can be simplified [Featherstone 1983, Hollerbach and Sahar 1983]. Since position and orientation can be decoupled at the wrist we can determine the velocities of the first 3 joints in one step and the velocities of the latter 3 joints in the next step. Since the reverse velocity determination is not computationally
intensive for the class of manipulators with spherical wrists, it is possible to determine the splines for each lookahead block in realtime.
With position and velocity constraints at *the start and end points of a block, velocities at.each intermediate point of the block can be estimated by imposing continuity in acceleration and solving the tri
diagonal matrix equation. If the block size is small, no matrix manipulations are involved since the equations for the intermediate velocities can be expressed in closed form. Thus, with position and
velocity constraints at each intermediate point of the joint trajectory, and ignoring the acceleration at the start and final points, cubic polynomials can be fit for the entire trajectory. The first and last polynomials will be quartics if initial and final point acceleration is considered. The interpolating curve will be continuous in the first and second derivatives at all points except the block end points where continuity in second derivatives is lost. We now briefly outline the equations for generating the spline coefficients for each block. For simplicity, a 2point lookahead is assumed.
A normalized time "u" for each segment of the joint trajectory is defined as follows:
For the ith segment, let
u : (t ti_)/(Ti_z)
where
Ti t1  ti_1 (2.34)
All polynomials are expressed with respect to u.
du/dt = 1/Ti.I on time segment i (2.35)
Let fi (u) = a3i u3 + a2i u2 + ali u + aoi (2.36)
For the ith segment, fi(u) is evaluated from u = 0 to u = 1.
dfi(u)/du = (dfi(u)/dt)/(du/dt)
= Ti_ * (dfi(u)/dt) (2.37)
Also, d2 (fi(u))/du2 = T2i.1 * (d2fi(u)/dt2) (2.38)
Equations (2.37) and (2.38) transform the velocity and acceleration of the joint at ti to velocity and acceleration of the normalized trajectory.
Let fl(u) = a31 u3+ a21 u2+ all u + a01 = 0 (2.39)
and f2(u) = a32 u3 + a22 u2 + a12 u + a22 = 0 (2.40)
be the cubic polynomials describing the trajectory between P1 and P2, and P2 and P3 in Figure 2.4.
Let 6l, 63 denote the velocities at P1 and P3 respectively. We know 61 from the polynomial preceding Pi. If P1 is the start point of the trajectory and the arm starts from rest, then 61 = 0. We can determine 63 by a "reverse velocity" transformation at time instant t3 (point P3). Splining (2.39) and (2.40) together with continuity in acceleration at P2, we derive an expression for 62 as follows:
(6a31 u + 2a21)/T~ u = (6a32 u + 2a22)/T~ u 2 0 (2.41)
Expressing all coefficients in (2.39) and (2.40) in terms of 01, 62, 63, 61I 02, 03, we get
a01 6 81 , all = 61T, (2.42) a21 = 3 (62  81)  (62 + 261)TI (2.43) a31 = 2 (e2  eI) + (61 + 62)T1 (2.44) a02 = e2 , a12 = 62T2 (2.45) a22 = 3 (63  e2)  (63 + 262)T2 (2.46) a32 = 2 (83  e2) + (62 + 63)T2 (2.47) Substituting in Eqn. (2.41) and solving for 62
62 = 1/2 (TI + T2) [(3T2/TI)(62  81) +
(3TI/T2)(e3 82)  1T2  '3T1] (2.48) The coefficients of the two cubic polynomials are computed from Eqns. (2.42) through (2.47).
An advantage of using actual joint velocities as constraints at certain intermediate points is that each joint will closely track the actual velocity required of it to maintain a desired end effector
velocity. This implies that the velocity profile of the end effector between intermediate points and at intermediate points will be closer to the desired value. As mentioned, acceleration continuity is lost at the
block end points.
For some manipulator geometries, an inverse velocity computation may be computationally too intensive for a limited point lookahead, online trajectory. A second formulation is proposed to estimate an
additional constraint at the block end points. The computations are independent of manipulator geometry and are simple enough for online implementation.
Scheme 2
The simplest approach to determination of an additional constraint at block terminal points is to compute joint velocity at that point by a linear interpolation. Thus, at the point P3, in Figure 2.4, the joint velocity can be estimated as
P3' = (P3  P2) / T2 (2.49)
Again, in this case, acceleration will be discontinuous at the block end points (including the start and final points). Since the estimation of the velocity by Eqn. (2.49) is local to the specific block, the tracking error will be larger. The "locality" of an estimation of this type is illustrated in Figure 2.5. Figure 2.5 shows a plot of a joint trajectory with 7 points. The 5 intermediate points are spaced equidistant in time. The joint trajectory is for joint angle 4 of the robot shown in Figure 2.2 for a linear end effector move. The
dashed trajectory is obtained by computing the 6 cubic splines by the conventional "offline" scheme. The slopes at the 5 intermediate points
for the offline scheme were determined by solving a 5X5 tridiagonal matrix. The solid curve is obtained by estimating velocities at every other point (2 point lookahead) by Eqn. (2.49). The plots demonstrate
e4 (radians)
 Offline
 Online
0.209
0.279
0.349
0 100
Figure 2.5
time (ms)
200 300 400 500
Comparison of offline and online cubic splines. Velocity estimation by linear interpolation
0
0.069
0.139
the tracking error resulting due to a poor estimate of the slope (Eqn. (2.49)) at the intermediate points.
A significant improvement in the estimate of the slope at the block
end points is achieved by the following technique. The idea is to estimate the slope at the block end points by overlapping successive blocks. An initial estimate of the slope at the block end points is made by assuming the last polynomial of the block to be a quadratic. This slope is then improved by "overlapping" the next block with the current block. The overlap is made between a point following the block end point and the point preceding the block end point. The improved estimate of the slope at the block end point is obtained by splining 2 polynomials between the preceding point, block end point and the next lookahead point with continuity in acceleration at the block end point.
Referring to Figure 2.4, the slope of the curve at point P2 (midpoint of the 3 point block) is initially estimated. Since the slope at P3 is not known, the slope at P2 is determined independently of the slope at P3. This is done by tying together a cubic polynomial between P1 and P2 and a quadratic polynomial between P2 and P3 with continuity in acceleration at P2. Thus, by making acceleration continuous at P2,
the slope of the curve at P2 is determined irrespective of the slope at P3. Now when point P4 is known, the slope at P3 is computed again using the same formulation as above. This new slope allows a cubic polynomial to be fit between P2 and P3 instead of a quadratic. Also, the new slope
at P3 is an improvement over the one computed by differentiating the quadratic. Thus, an improvement in the estimation of the slope is
obtained by overlapping successive blocks. Let
fl (u) = a31 u3 + a21 u2 + all u + a01 = 0 (2.50)
f2 (u) = a22 u2+ a12 u + a02 = 0 (2.51)
be the equations of a cubic polynomial between P1  P2 and a quadratic polynomial between P2  P3 (Figure 2.4). The normalized time variable is denoted by "u" as before. The initial conditions at P1. P2 and P3 are shown in Figure 2.4, except in this case, there is one less boundary condition at P3; that is, 03 is not known (recall that in Scheme 1, 03 was initially determined by a "reverse velocity" computation). Hence, f2(u) is given by a quadratic equation (2.51).
Splining (2.50) and (2.51) together with continuity in acceleration at P2, 02 is determined as follows:
6a31u + 2a21/T12 = 2a22/T22 (2.52)
u=1 u 0
Expressing all coefficients in (2.50) and (2.51) in terms of 81, 01, 2, 02, and 03 ,
a0l = 1 , all : 1Tl (2.53)
a21 = 3 (02  01)  (261 + 62)T1 (2.54)
a31 = 2 (02  01)+ (01 + 62)T1 (2.55)
, a12 = 62T2
a02 = 62
(2.56)
a22 = (03  '2)  62T2
Substituting in Eqn. (2.52), we evaluate 62 as
02 = (TIT2/(2T2 + TI)) When point P4 is polynomial between P2 03 = (T2T3/(2T3+ T2))
* [(1/T )(e3  02) + (3/T1)( 2  61)  ;1/T,3 (2.58)
known, 63 is determined by splining a cubic P3 and a quadratic between P3  P4.
* [(1/T )(34 83) + (3/T2)( 3  02)  ;2/T2] (2.59)
With the new value of 63 given by (2.59), coefficients of a cubic polynomial between P2  P3 are computed (instead of the original quadratic) as follows:
a02 = 02
, a12 = 62T2
a22 = 3 (03  82)  (262 + 63)T2 a32 = 2 (83  e2) + (62 + 53)T2
(2.60) (2.61) (2.62)
Thus, the coefficients of the cubic polynomial between P1  P2 are determined from Eqns. (2.53), (2.54), and (2.55) with a knowledge of e, 61, 62 and 03; and the coefficients of the cubic polynomial between P2 P3 are determined from Eqns. (2.60), (2.61), and (2.62) with a knowledge of 82, 02, e3, 03 and 04 (point P4).
(2.57)
The improved tracking accuracy of this scheme is demonstrated in Figure 2.6. The plots of Figure 2.6 show the same joint move as Figure 2.5. The dashed trajectory is the "offline" spline solution. The solid trajectory is obtained by a 2 point lookahead on the joint trajectory with slope determination as described above.
In order to illustrate the improvement in trajectory with the number of lookahead points, Figure 2.7 shows a plot of the joint trajectory of Figures 2.5 and 2.6 with a 4point lookahead. In the preceding section, we derived that a lookahead of 4 points for an equidistant knots cubic splines trajectory produces a system memory error of less than 0.5 percent. We deduce from Figure 2.7 that the piecewise splines track the offline splines trajectory with almost no undershoot or overshoot, illustrating better tracking due to negligible coupling in the determination of the velocities at the block end points of successive computational blocks.
In Eqn. (2.59), the slope at P3 was determined with the overlap in the succeeding block limited to a single lookahead point (P4). With this technique, joint trajectory interpolation can start with a 2point lookahead for the first block, after which, only 1 lookahead point on the trajectory is required to compute the next joint polynomial. Thus, this scheme is adaptable to sensorbased control where smaller lookahead means quicker adaptation to the new trajectory.
The estimate of e3 by Eqn. (2.59) can be further improved by employing both points of the next lookahead block (2point lookahead) to compute 03 . In this case, we spline two cubic polynomials between P2  P3 and P3  P4 and a quadratic polynomial between P4  P5, with continuity in the second derivative.
Offline Online
0 100
Figure 2.6
0
0.072
0.144
0.216
0.289
0.361
64 (radians)
S, ,  time (ms) 200 300 400 500
Comparison of offline and online cubic splines. Velocity estimation by 1point overlap, 2point lookahead
(radians)
 ffline
Onli ic
Figure 2.7
.027
0
0.069
0.139
0.209
0.279
0.349
t ilie (ills)
200 300 400 500
Comparison of offline and online cubic splines. Velocity estimation by 1point overlap, 4point lookahead
The acceleration continuity equations at P3 and P4 are
(6a32u + 2a2)/T2 (6a33u + 2a23)/T2
IU =
= (6a33u + 2a2/T2 = 2a24/T2
Substituting for
the coefficients aij in Eqn.
(2.63) and
simplifying, we get
203 Il/T2) + (1/T3)} + /T (3/T2) (04
Substituting for the coefficients aij
in Eqn.
simplifying
(e3/T3) + 64 {(2/T3) + 1/T4)} = (T)  64)+ (3/T) (64
Solving for
o3 and ; 4
(2.64) and
 63)
(2.66)
from Eqns. (2.65) and (2.66),
63 = [{T2T3 (T3 + 2T4)} / {2(T2 + T3) (T3 + 2T4)  T3T4}3
[(3/T 2) (04  02)  {3T4/T2 (T3+ 2T4)} (64  03)
1I/T4 (T3 + 2T4)1 (05  64)  ;2/T2]
04 :{T3T4/(T3 + 2T4)} [(I/T4)(e5 e4)
+ (3/T2) (04  3)  T (2.68)
lu = 0
(2.63) (2.64)
 62)  ;2/T2
(2.65)
(2.67)
We note that the overlap correction causes a discontinuity in
acceleration due to the improved velocity estimate at the block end point. For a 2point lookahead, and velocity estimations by Eqns. (2.58) and (2.59), acceleration is discontinuous at the block end point and the point preceding the block end point. If the velocity at the block end point is determined by Eqn. (2.67), then acceleration continuity is lost at the point preceding the block end point. In the next section, we show that the discontinuity in acceleration due to the
online cubic splines employing overlap velocity correction is, to a first approximation, always smaller than the acceleration jumps in an online cubic splines without overlap velocity correction.
Acceleration Jump in OnLine Cubic Splines
The acceleration jump at the "join" point of two cubic polynomials, fi(u) and fi+l(u) is given as
[Aa]i = [(6a3iu  2a2i)/Ti 2]  [(6a3(i+l)U  2a2(i+l)/Ti+2 u=l +1u=O
(2.69)
where
fi(u) = a3iu3 + a2iu2 + aliu + aoi (2.70) u3 u2
fi+(U)= a3(i+l) u+ a2(i+l) + al(i+)U + ao (i+1) (2.71)
For simplicity, we assume equal time intervals; that is, Ti Ti+I = T . Substituting for the cubic polynomial coefficients in t(
terms of 8, 8 (as in Eqns. (2.42)  (2.44)), in Eqn. (2.69), we obtain
[Aa]i+I = (1/T2) [ 6 (6 i+  6i ) + (26i + 46 i+)T
6 (0i+2  ei+1  2 (ei+2 + 2ei+1)T] (2.72)
[Aa]1 +  2  3 (e  i) + T(1 + 46i+1 + i+2)] (2.73)
Equation (2.73) gives the jump in acceleration at the point where two cubic polynomials are splined together.
In order to estimate the acceleration jump in the online cubic
splines approximation, we assume that the actual joint trajectory is continuous and nth order differentiable, where n could be as large as desired. This assumption is not unreasonable for continuous motion of the robot end effector since the end effector trajectory is a composition of the joint trajectories (applying the well known theorem, the composition of continuous functions is continuous and vice versa).
The acceleration jump will be estimated in terms of the derivatives of the continuous nth order differentiable joint trajectory.
We initially estimate the acceleration jump for an online splines
joint trajectory with no velocity correction at the end points of each computational block. For a 2point lookahead (referring to Figure 2.4
again for the example trajectory), the velocity at the block end point
(P3) is simply the derivative of the quadratic polynomial between P2 P3.
Let 6i., ei, 6 i+ and 6i+1, 6i+2, 6i+3 denote 2 successive computation blocks. We will estimate the jump in acceleration at the (i+l)st knot. The velocities ;i+1 and 8i+2 are
9i+1 : (2/T) (i+1
 8i)  ei
i+2 = (1/3) [{(8i+3  ei+2)/T} + 3 1(ei+2  Oi+l)/T} ei]
The acceleration jump at the (i+l)st knot is
[Aali+1 = (2/T2) [ 3 (8i+2  ei) + T (; + 4;i+1 +0i+2)]
= (2/T2) [ 3 (ei+2  ei) +
T L;i + (8/T) (6i+1  i) 48i +
{(Oi+3 . 1i+2)/3T[ + (ei+2 + ei+l)/T  ei}] (2.76)
Rearranging terms in Eqn. (2.76),
[Aa i+I = (2/T2) [(1/3) [6  ei)  (7/3) (ei+2  ei) +
(19/3) (6i+I  ei)  (8/3) ei] (2.77)
Expanding the terms in Eqn. (2.77) by Taylor series about 6i we get
(ei+3  ei) =
(3T) + 61 (3T) 2/2 + 6 (3T) 3/6 +
1 1
6(iv) (3T)4/24 +
1
6.i+2 e) = eI (2T) + e11 (2T) 2/2 + 8.ll (2T) 3/6 +
8(iv) (2T)4/24 +
1"ï¿½
(2.74) (2.75)
(2.78)
(2.79)
ei+ i) el (T) + Oil T2/2 + e111 T3/3 + 6(iv) T4/4 + io 1 1 T/4+ "
From Eqns. (2.78), (2.79), (2.80), and Eqn. (2.77)
[Aa]i : (2/T2) [ 0.5555 T3 6111  0.16667 8!iv) T 4 _ 0(T5)]
i+1 1 I
:  [(1.111 T) e + (0.3334 T2) e6iv) + 0(T3)]
"1 1
(2.80)
(2.81)
We now estimate the acceleration jump due to velocity correction at the block end points by Eqn. (2.59) which assumes a 1point overlap. The velocities 8i+1 and 6i+2 for this case (again assuming a 2point lookahead) are given as
0i+I = (1/3) [(6i+2 + ei+l)/T + 3 (0i+I
 ei )/T  0i]
(2.82)
(2.83)
; i+2 = (1/3) [('i+3  0i+2 )/T + 3 (ei+2  0i+l)/T  ;i1l
Substituting Eqns. (2.82) and (2.83) in Eqn. (2.73) and simplifying
[Aa]i+ = (2/T2) [(ei+3  ei)/3  10 (0i+2  ei)/9 +
13 (ei+  8i)/9  2T ;i/9]
(2.84)
Expanding the terms in Eqn. (2.84) by Taylor series as before and simplifying
[Aa]i+I = (2/T2) [(0.2592 T3) ill + (0.444 T4) e iv) + 0(T5)] (2.85)
1 I
[Aa]i+ = (0.5185 T) 6 + (0.8888 T2) 8!iv) + 0(h3) (2.86) 1~ 1
Finally, we estimate the acceleration jump for velocity correction by a 2point overlap as in Eqn. (2.67). Note that in this case, there is no acceleration jump at the block end point. The jump in acceleration is at the point preceding the block end point. Therefore, with the two blocks i1l, a i+I and ei+1 0i+2, 8i+3 , the accelerations jump is at the ith knot. The velocities 8i and 8i+1 are
8i = (I/T) [(Oi+1  a0)/3 + (8i  il)]  0i_i/3 (2.87)
ei+1= (1/11T) [ oi+3 + 7 8i+2 + 2 8i+ . + 3 0i_] + 0i_1/11
(2.89)
[Aa1i = (2/T2) [ 3 (ei+I  i.I) + (Il + 4o.+ e +)] (2.90)
[Aa]i = (2/T 2) [(6i+3  8i1)/ll + 7 (0i+2  eii)/ii
49 (8i+I  ei_)/33 + 5 (8i  8i_1)/3  8 T 8i_1/33] (2.91)
[Aali = (0.3838 T) 811 + (0.515 T2) 8(iv) + 0(T3) (2.92)
Thus, Eqns. (2.81), (2.86), and (2.92) give the equations for the acceleration jumps due to online splines employing no velocity
correction, and velocity correction with a 1point and 2point overlap respectively at the block end point. To a first degree of approximation on T, the magnitude of the acceleration jumps due to the overlap schemes
are always smaller than the acceleration jump due to online splines with no velocity correction at the block end point. As "T" increases, the second order terms show that the increase in the magnitude of the acceleration jump is more rapid in the splines with the overlap correction (Eqns. (2.86) and (2.92)).
Results of Simulation and Experimentation
In this section, the performance evaluation by simulation and experimentation of Schemes 1 and 2 of the previous section, is presented.
Online cubic splines approximation of joint trajectories provides the servo with a smooth joint displacement and velocity. It also ensures better tracking of the end effector trajectory. The tracking accuracy of the splines is shown for a particular Cartesian move by the plots (Fig. 2.8) of the end effector path error in mils (1/1000 of an inch) versus path velocity for 3 different transform update rates. The transform update rate in milliseconds is the time interval between successive joint coordinates. Experimental plots for a variety of robot moves not involving singularities and limit points demonstrate characteristics similar to those in Figure 2.8. The length of the linear end effector move used in our example simulation was I meter. The average path deviation for the entire path for different velocities and transform update rates are plotted in Fig. 2.8 for this move. For comparison, Figure 2.8 shows the average path error for a path velocity and a transform update rate when a linear function is used for joint trajectory interpolation (as in Figure 2.3) and cubic splines of Scheme 2 are used for interpolating the same joint trajectory. From Figure
61
path
deviation
(mils)  Linear interpolation in joint space cubic splines with
2 point lookahead
20Z
:200 ms
/ update
15ï¿½" 0 Ms .oï¿½'" update
I //
' / I I
150 / 00 rrs / update
10, i l / 150 /
/ / /
[ /J/
 , /
150 ms
update
. update
0 2040 6 60path velocity (in./sec)
Figure 2.8 Maximum path deviation vs. path velocity and update rates for linear and cubic splines interpolation of a typical Cartesian move
2.8, it is seen that for all transform update rates for this particular motion, the cubic splines track the end effector trajectory with greater accuracy than the linear interpolation of joint coordinates.
The splines cause a small overshoot at points on the joint trajectory where a change in direction occurs. Experimentation with a large number of linear end effector moves has shown that the error in end effector trajectory due to a small overshoot in one or more joint angles is insignificant, unless operating close to a joint limit. Figure 2.9 shows a joint trajectory which is almost linear with time. Cubic splines of Scheme 2 with a transform update rate of 100 ms are used for interpolating the joint trajectory. Notice that the cubic splines track this trajectory with no overshoot.
Both Schemes 1 and 2 were tested on a 6 degrees of freedom robot, the kinematic structure of which is shown in Figure 2.2. Experimentation with a range of transform update rates from 50ms to 200ms showed that the cubic splines produced smooth motion of the joints during a Cartesian motion of the end effector. Linear interpolation of joint coordinates showed visible jerkiness in motion for update rates higher than lOOms.
T1hETA
.175 .157 .14.
. 122
* f117.
. 052 .135 .017.
'I
Figur
C i V I V I V .275 .413 .550 .no8 .1020 .063 1. 111 1.23 7111r. (MS)
e 2.9 Online cubic splines showing no overshoot for an
almost linear joint trajectory
0 .137
1. 37
V
CHAPTER III
ROTATIONAL SPACE CURVES FOR CONSTRUCTION AND COMPLETE
DESCRIPTION OF ROBOT TOOL ORIENTATION TRAJECTORIES
In Chapter II, we developed novel techniques for the construction of joint space trajectories for both joint interpolated motion and constrained tool motion of the robot manipulator. In order to accurately track the end effector path by the online approximation of the joint trajectories, the controlled end effector trajectory in
cartesian space must conform to the "feasibility constraints" of the manipulator. Feasibility constraints on the tool motion are the maximum tool velocity and acceleration given by the physical bounds on the velocity and acceleration of the individual joints. Since the tool is translating as well as rotating about an axis in space, both translational and rotational motions of the tool must satisfy the feasibility constraints. The current approach to planning an end effector motion is "translationdominant" [Paul 1981, Taylor 1979]. A translation dominant approach is the planning of a trajectory based on the translational move parameters. The rotational trajectory is constructed from the move time of the translational trajectory. For those motions of the end effector where there is a greater rotation than translation, as in the rotation of the tool in the tool frame, translation dominant techniques may be deficient or even fail to produce a continuous and "feasible" rotational motion of the manipulator. Again, by "feasible" rotational motion, it is meant that the rate of
change of the tool rotation and the acceleration of the change is
limited by the maximun rate and acceleration constraints on the joints of the manipulator. Also, a translationdominant move makes the
rotational error in moves such as those requiring an instantaneous turn of the robot tool, proportional to the translational move parameters. Since translation and rotation are independent motions, we introduce, develop, and describe the techniques for the independent determination of the tool rotational trajectories in this Chapter.
End Effector Motion Specification and Trajectory Planning
It is common in practice to specify the translational trajectory of
a robot tooltip to the required degree of exactness for a given robot task in 3space. Since the tool, in general, also rotates along its translational space curve, and rotation and translation are independent motions, a complete specification of the rotational trajectory is in order. Emphasis on rotational trajectories has been underplayed in robot motion since it is difficult to visualize and specify an exact
tool orientation trajectory. Therefore, as long as the rotation is uniform and smooth along the translational tool trajectory, no
constraints are imposed on the specific nature of the orientation trajectory.
The simplest tooltip trajectories in Cartesian space are either linear or a composition of linear paths. Along a point to point linear path, there are two wellknown methods of rotation interpolation. One approach is to determine an axis in space such that the tool rotates about this axis with constant velocity and the total rotation about this
axis transf6rms the initial tool frame to the final tool frame [Taylor 1979]. A second method described in [Paul 1979] uses the freedom of
choice of the rotational space curve to split the total rotation about the linear path into 2 independent rotations. One rotation aligns the tool vector from an initial to the final orientation, and the other rotation is about the tool axis itself, to align the faceplate vectors. The two rotations are interpolated linearly between the start and destination points. The intermediate orientations of the tool on the linear path for Paul's and Taylor's schemes are generally different. The nonexistence of a unique rotational trajectory illustrates the fact that the rotational motion of the tooltip on a cartesian path is underspecified. One of the objectives of this Chapter is to define additional constraints on the rotational space curve to resolve some ambiguity in interpolating the tool orientation along any Cartesian space curve of the robot tool. Two approaches are described for the specification of constraints on the rotational space curve. The first one is based on a description of the relationship between the rotation space curve and the translation space curve. The second approach is to completely specify the rotational curve in rotation space only.
For most point to point moves, a uniform tool rotation is desirable due to smaller inertial forces on the tool. If the objective is to simply transform the tool smoothly from one orientation to another, as in a tool transiting between work points, or tracking an object based on sensory feedback, the only constraint on the tool motion is smoothness. We term this type of tool motion unconstrained. Another example of unonstrained tool rotation is during the transition of the tool translational path from one linear segment to another. The only
constraints on the tool rotational trajectory in a motion involving a transition from one linear segment to another is the continuity in
position and velocity at the 2 boundary points. Most of the existing techniques for a smooth unconstrained tool rotation employ a "translation dominant" approach for computing the time for the tool rotational transition. The translation dominant approach is to compute the move time based on the translational parameters (linear velocity and acceleration) of the tool. For some robot moves where the translational displacement is much smaller than the rotational displacement of the robot tool, a translation dominant approach may cause excessive acceleration demands on some joint servos of the manipulator. In this Chapter, we separate the translational and rotational motion by decoupling the manipulator degrees of freedom. A common wrist for many 6 degrees of freedom (DOF) industrial manipulators is the spherical wrist. In a spherical wrist, the 3 axes of the degrees of freedom from the wrist to the tool intersect at the wrist joint. For manipulators with spherical wrists, we derive a simple method for the independent determination of the rotational move parameters by decoupling the manipulator DOFs at the wrist into translational and rotational DOFs.
There are some robot tasks that require constrained changes of the robot tool. For example, in an arc welding task, the orientation of the torch with respect to the work surface is always of primary concern.
Thus, in case of an orientation change, the torch angle with respect to the work must be maintained constant. This type of orientation change is termed constrained. Motions with constrained tool orientation are executed by transforming the constraints into joint coordinates at every sample time instant. A simple method of determining the maximum constraints (velocity and acceleration) on the constrained rotational motion is given in later sections.
In order to construct a continuous rotational space curve for an
offline trajectory, a 3dimensional "rotation space" is defined. A rotation space is defined by rotations about three orthogonal axes of a fixed frame, such that a point in the frame represents a tool orientation. The order of rotations about the axes of the rotation space is fixed. Therefore, either the roll, pitch and yaw angles or the three Euler angles can be used for creating the rotation space with respect to the fixed frame. For most articulated 6degrees of freedom robot manipulators, the first three joints determine the wrist position and the last three joints the tool orientation from the wrist. There are different types of wrist joints in articulated manipulators. We can classify the wrist joints into 2 common types, an Euler wrist (spherical
wrist) and an RPY (roll, pitch, yaw) wrist. The choice of the specific rotation space is based on the type of wrist of the manipulator.
In the rotation space, well known curve fitting techniques can be employed for creating a smooth rotational space curve through a number
of via points. Also, transition from one rotational space curve to another in rotation space can be conveniently smoothed to any degree by
employing polynomial smoothing techniques. A continuous (at least in the first time derivative) rotation space curve not only gives a unique rotational trajectory for a move, but also guarantees a smooth
transition from one velocity to another without having to stop at the point of transition.
The next section describes the construction of rotation space.
Definition of a Rotation Space
A continuous rotation space can be created by choosing 3 independent rotations of the tool about a fixed base or universal frame. Thus, roll, pitch and yaw; or the 3 Euler angles can be employed to create the rotation space. The basis of the rotation space will be 3 orthonormal axes, each axis representing the roll, pitch or yaw, or the
3 Euler angles about the axes of a predefined base or universal frame. Any orientation of the robot tool maps to a point in the rotation space. The space curve of the points in rotation space for a given move is called the rotation space curve.
We use standard definitions of roll, pitch and yaw or the Euler angles. The roll is defined as the rotation about the zaxis of the base frame, the pitch as rotation about the yaxis and the yaw as the rotation about the xaxis. The rotation matrix transforming the base frame to the tool frame for a RPY point ( *, 8, ' ) in rotation space can be derived [Paul 1981] as
bRt = Rot ((z, ), (y,e), (x,*))
. cos * cos 6 cos * sin 8 sin ' sin * cos
 sin * cos 6 sin @ sin o sin '+ cos 1 cos
sin cos 0 sin '
cos * sin e cos '+ sin 0 sin 'p
sin * sin e cos '  cos * sin
cos 0 cos
(3.1)
Similarly, the transformation of an Euler point ( i, , p) is given by the following transformation matrix:
bRt = Rot (z,4)) Rot (y,e) Rot (z,p)
[cos cos 6 cos 4 sin 4sin cos os sin sin cos 4 = {sin ï¿½ cos 0 cos + cos ï¿½ sin 4  sin 4 cos 0 sin 4+ cos 4 cos
L  sin 0 cos 4 sin 8 sin *
cos 4 sin 01
sin * sin 6
Cose J
(3.2)
The matrix bRt is a transformation relating the base and tool frames. In the following sections, methods for unconstrained and constrained rotational transitions are described. The main emphasis is on separating the determination of the move parameters for translational and rotational parts of a motion.
Specification of Constraints on a Rotational Trajectory
All rotational transitions can be classified into constrained and unconstrained rotations of the manipulator tool. In this section, we
classify the constraints on a rotational trajectory into two types of constraints called explicit and implicit constraints.
Let us consider the simplest end effector move in Cartesian space, a point to point linear move. A point to point linear move is defined by specifying the position and orientation (referred to as pose) of the manipulator at 2 points in the work space of the robot. The translation
interpolation is straight forward since the desired trajectory (linear) is specified. However, the rotational space curve has no constraints in the move specification. Constraints on the rotational space curve can be specified in relation to the translational space curve or
independently. A description of the constraints to be specified for a welldefined rotational space curve are classified into explicit constraints and implicit constraints. Explicit constraints directly specify the nature of the rotation space curve by giving a number of intermediate target points. Implicit constraints specify the rotational trajectory as a function of the translational trajectory.
Explicit Constraints on the Rotational Trajectory
Let us assume that on the linear path, the tool orientation is given at one or more intermediate points. The larger the number of points in rotation space for a move, the better the approximation to the rotational space curve (in general). This form of constraint specification is termed explicit, since interpolation points are directly available in the rotation space and the rotational space curve can be approximated by a polynomial interpolation through the intermediate points.
Wellknown polynomial interpolation techniques can be'employed for constructing the rotational space curve from a number of via points in rotation space. If only 2 points (start point and destination point) in rotation space for a point to point linear move are given, then one solution would be to approximate the rotational trajectory by a quintic polynomial, since a total of 6 constraints (position, velocity and acceleration) are known at the 2 end points. For the quintic approximation in rotation space, the direction and magnitude of the tool rotational velocity will not be constant, and the intermediate tool orientations with respect to the translational curve will be unpredictable.
If the rotational motion is to be at a constant angular velocity, then appropriate via points (for interpolation) in the rotation space must be specified in order to generate the correct space curve.
Implicit Constraints on the Rotational Trajectory
Implicit constraints define the rotational motion with respect to
the translational tool tip trajectory. For example, one can specify the tool vector always be along the normal to the tool tip trajectory. A typical application where the robot tool tip may be specified along the normal to the translational space curve, is tooltip motion along a circular arc. If the rotation about the tool axis is given as uniform,
then the robot tool traces the circular arc with the tool orientation always along the normal and uniform rotation about the tool vector. Implicit constraints must be transformed to generate intermediate points in the rotation space such that a rotation space curve can be generated.
Another implicit constraint on tool vector orientation can be specified with respect to the initial and final tool vector orientations. The constraint could be that the intermediate tool orientations must lie in the plane formed by the initial and final tool
vectors (both vectors expressed as unit vectors at the origin of the tool frame at the start point). This constraint is used in Paul's scheme for generating the tool rotation [Paul 1979, 1981]. An advantage of imposing the "coplanarity constraint" on intermediate tool vector orientations is illustrated by an example. Assume that a linear move of the robot end effector is programmed such that the initial and final tool vectors are coplanar. If the equivalent axis of rotation approach [Taylor 1979] is used to interpolate rotations, then there is no guarantee that the tool orientation will lie in the plane of the initial
and final tool vectors. Thus, the constraint that the tool always lie in the plane formed by the initial and final tool vectors restricts the freedom of the rotational space curve. There is, however, the choice of
the curve within the plane of the 2 tool vectors for the intermediate orientations. If the rotations are to be uniform, then this curve will be linear.
Any constraints on the tool vector in Cartesian space have to be transformed into rotation space as interpolation points. Thus, for a point to point move, if the tool vector is constrained to a certain trajectory, each sample point on this trajectory has to be transformed into the rotation space angles. Polynomial interpolation through these sample points for the angles in rotation space yields the rotation space trajectory for constrained tool motion. For an offline creation of a
rotational trajectory, interpolation points must be determined in the rotational space based on a technique like the bounded deviation joint paths method [Taylor 1979] that minimizes the rotation error to a certain specified value.
In the next section, we describe a conventional translationdominant approach for planning rotational transitions. Since translation and rotation are independent motions, we derive a method for planning independent rotation transitions in the following sections. In order to decouple the rotational motion from the translation, we need an
estimate of the rotational tool acceleration in the robot workspace. Equations for the rotational acceleration are developed for a special class of manipulators with spherical wrists.
Construction of Rotational Trajectories  Conventional Translation Dominant Approach
In this section, we describe some of the disadvantages of a translation dominant approach for computing the rotational move. An example move is chosen to demonstrate the effect of planning a rotation trajectory based on the translational trajectory parameters. The example move is a transition of the robot tool from one constant velocity linear segment to another. In this case, the points for starting and ending the transition must be determined. The translation dominant approach employs the constraint on the maximum linear acceleration of the tool to produce a symmetric transition of the tooltip. The same time period is used for the rotational change also. It is shown that the rotational error, defined as the rotational deviation from an intermediate point that the tool misses due to the continuous transition, is directly proportional to the transition time. Thus, if the translation transition time is high, the rotational error will be large, although the rotational change may be small between the points.
In Figure 3.1, let P1 be the point where the robot starts from rest, moves linearly with a constant velocity v, to point P2, turns instantaneously at P2, moving linearly with a constant velocity v2 to point P3. From the tool orientations at P1 and P2, an axis of rotation, k 1 for rotating the initial tool frame at P1 uniformly into the frame at P2 is determined [Taylor 1979] (see [Paul 1981] for derivation of k 1 ). Another axis of rotation, k 2 9 is determined for rotating the frame at P2 into the frame at P3. The move specification requires that the robot tool turn instantaneously at P2 without stopping. Since this cannot be done physically, such motion must be
K 2'
A segment to segment transition
vi 9 Tj/
Figure 3.1
approximated. An accepted method of turning the tool at P2 is to execute a "flyby." A flyby begins turning the tool before P2 such that the tool misses the point P2 but makes a smooth transition to the new segment P2P3 [Paul 1979, Taylor 1979]. The degree of the polynomial for the segment to segment transition of the tool tip is reduced by making the transition curve symmetric with constant linear acceleration. The time for the transition is determined from the two linear velocities, v 1 and v 2' and the maximum linear tool acceleration, amax, as follows:
2T= 1  v 2 I/amax (3.3)
where 2T is the time for the transition.
Regardless of the orientation change required between the two segments, the transition time 2T determined from the translational motion parameters is also employed for the rotational transition. The main advantage of using the translational transition time for the rotational change is the simplicity in executing the motion, since the translational and rotational transitions begin at the same time and end at the same time. The tool orientation is required to change from the axis of rotation k 1 and velocity w 1 . to the axis of rotation k 2 and velocity w 2 * The following equation produces a smooth transition (unconstrained), keeping the orientation and rotational velocity continuous at both ends of the transition.
( t')2 'k (t+ t')2
R(t) = R2. Rot (k i (  at 1) . Rot (k 2'(4T + 2) (3.4)
where t' = t  T1, T1 being the move time for the segment P1P2 and R2 is the rotation matrix at the "flyby point" P2. The rotation matrix R(t) after T seconds into the flyby segment (t = TI) is given by
R(TI) =R2 . Rot (k 1'  WlT/4) . Rot (k 2' w2 /4) (3.5)
If the robot end effector were to actually pass through the intermediate point P2, the rotation matrix R(t) at t = T1 seconds should be R2, the orientation at the point P2. If ( n 1, o 1, a 1) denote the 3 column vectors of R(TI) (Eqn. (3.5)), and (n, o, a) denotes the column vectors of the actual rotation matrix at P2, then the orientation error is defined as (see [Luh 1980b] for derivation)
= [(n x n ) + (o x o ) + (a x a (3.6)
In order to estimate the rotation error a as a function of T for Eqn. (3.5), let Ae1 =  WlT/4, and Ae2 = W2T/4. For small error analysis, we assume that A6 and A82 are small. Expanding Eqn. (3.5),
F 1 k Ae ky 81 1 k z2Ae k y2A6 R(TI) = Rz2 kz2 1I 1 .kX1 AeI kz2 A22 1 kx21A02
kyl Ak 1 k xl Ae1 1 k2 A02 kx2 A82 1
1
k AeI + kz2 Ae2
kyI A81  ky2 Ae2
k Z1 Ae  kz2 te2
1
k xl AeI +k kx2 A82
ky1 AO1 + ky2 AO2
kx2 A62 kX1 A81
1
(3.7)
All second order terms in
eI = kZ1 AE e2 = kyI AE e3 = kX1 AE
Ae are ignored in Eqn. (3.7). '1 + k z2 A02 '1 + ky2 AG2 1 + kx2 A02
Eqn. (3.7) can be written as
R(T1) = R2 [
Let R2 = [n, o, a].
1 e1 e2] e 1 e3
e 2 e3 1
Substituting in Eqn. (3.11),
nx + el ox e2 ax R(TI) ny e1 Oy  e2 ay
nz + el oz e2 az
nx e1 + x + axe3
ny eI + oy + ay e
nz el + oz + az e3
nxe2 ox e3 + ax ny e2 oy e3 + ay nz e2 ï¿½ze 3 + az
(3.12)
(3.8) (3.9)
(3.10)
(3.11)
now derived from Eqn. (3.6).
(n 1 , o 1 , a 1 ) are the 3 columns of Eqn. (3.12). Taking the cross products and simplifying the terms we get
a = {eI. (n x o)x + e2. (a x n)x + e3. (ox a)x} i
+ {eI. (n x o)y+ e2. (a x n)y + e3. (ox a) y} i
+ {eI. (nxo)z + e2. (a x n)z + e3. (o x a))z kI_
[el a+ e2 o+ e3 E]
a . a a 1 2 = (el2 + e 2 + e3 2
(3.13) (3.14)
Substituting
AG1 =  I T/4 and AO2= W2 T/4 in
Eqns. (3.8) 
(3.10),
eI = (T/4)
(k zi 'l + k z2 '2)
= (T/4) (kyI W1 + ky2 W2)
e3 = (T/4)
(kxi 1 + kx2 '2)
Substituting Eqns. (3.15)  (3.17) in Eqn. (3.14),
2 (T2/16) (w  2w I 2
1 1 2
jli= (T/4) . (w2 2&~w2
k. k 2+ w2
2 + (3)l/2
(3.15)
(3.16)
(3.17)
(3.18)
The rotational
error a is
Therefore, the magnitude of the rotational error is directly proportional to the translational flyby time T, independent of the arm kinematic structure. A plot of ji versus T in Figure 3.2 illustrates the linear relationship between rotational error and T. The kinematic structure of the robot used for generating the plot of Figure 3.2 is shown in Figure 2.2. The move parameters for the plot are given in Table 3.1. A maximum linear tool acceleration of 1 m/sec2 was employed for the determination of flyby time "2T."
Table 3.1 Move parameters for Figure 3.2
JOINT COORDINATES (DEG.) LINEAR VELOCITY 1I e2 83 84 05 66
Start point P1 0 0 0 0 0 0 0 m/sec
Intermediate 50 0 0 90 45 0 .07 m/sec point P2
End point P3 100 45 45 0 90 45 .15 m/sec
Thus, the rotational flyby error for a rotation transition given by Eqn. (3.4) directly depends on the velocity gradient of the linear translational path, a dependence not justified by the independence of the translational and rotational motions. If T is independently
JIu Isin o1 .015
.012 .009 .006 .003
15 30 Figure 3.2
.1(.,s)
45 60 75
Rotational error vs. flyby time
determined for the rotational transition, the rotational error IaI can be made smaller and also independent of translational move parameters.
In order to determine a rotational flyby time, we need an estimate of the maximum rotational acceleration of the robot tool. Since the rotational acceleration of the tool is obviously a function of the robot
pose, a conservative value must be determined such that it is valid for the entire workspace of the robot. One way to determine the instantaneous rotational acceleration of the tool frame is to express the tool frame as a rotation about an axis in space. Let (k, 8) denote this axis and the rotation about the axis respectively. If the tool frame vectors are denoted as (n, o, a) , then 8 is given as
6 = tan1 (Oz _ ay)2+ (ax  nZ) 2+ (ny 0x) (319) n +o +a 1
x y z
T T T where n= (nx, ny, nZ) o = (ox, 0y, OZ) and a = (ax, ay, aZ) Since (n, o, a) are functions of the joint angles (81 .. 86), Eqn. (3.19) can be written as
= f (8 .. 86) (3.20)
Differentiating Eqn. (3.20) twice with respect to time, and substituting for the maximum values of the joint rates and accelerations, we can obtain a value for the instantaneous tool
acceleration when all the joints are driven at their maximum rates. Since the rotational acceleration is a function of the arm pose, the
magnitude of the rotational acceleration varies at different points in the robot workspace. The equation for the rotational acceleration
obtained by differentiating Eqn. (3.20) is a nonlinear function of the joint angles, velocities and accelerations with a large number of coupling terms in the joint parameters. Therefore, a mathematical estimation of a maximum magnitude of the rotational acceleration that is
usable in the entire robot workspace independent of the arm pose and velocity, is nontrivial by this approach.
In the following sections of the chapter, we analytically derive the limits on the tool rotational acceleration by decoupling the rotation into two rotations about the robot wrist.
Construction of Independent Rotational Motions
In this section we describe methods for producing independent tool rotations. In order to determine an independent move time for the rotations, we need an estimate of the maximum rotational acceleration as mentioned earlier. For certain unconstrained tool rotations, the limits on the maximum joint rates and accelerations can be directly employed to determine a rotational move time. For many other rotational moves, we need an estimate of the tool rotational acceleration, which is a composition of all the joint rates and accelerations. This estimate of the tool rotational acceleration must be valid for the entire robot workspace, independent of the arm configuration. We derive the equations for the estimation of the rotational acceleration of the tool. The derivation is specifically for the most common industrial articulated robot geometry  manipulators with spherical wrists. The spherical wrist decouples at the "wrist" joint of the articulated manipulator into the so called position and orientation degrees of freedom. This property is used to decouple the rotational motion into
rotation due to the orientation angles, and rotation due the positioning angles. The decoupling is employed for planning independent rotational motions for the orientation angles, 049 05 and 86 . An estimate of the rotational acceleration in the "rotation space" formed by the orientation angles is determined. Due to the simplification afforded by the decoupling, the exact nature of the rotational acceleration in the robot workspace is easily visualized and analytically proven. We
completely analyze one type of a wrist, namely the "Euler" wrist, for maximum rotational velocity and acceleration of the tool. The results of the analysis are applied to a simulation example that computes independent rotational transitions. The plots of the joint angles
versus time show the improvement in the joint trajectories due to an independent rotation trajectory computation as compared to a translation dominant approach for trajectory determination.
The next section describes the estimation of a rotational acceleration of the tool in the robot "rotation space." Planning Constrained Rotation Transitions Estimation of Acceleration For Spherical Wrist Manipulators
For a manipulator with rotary joints, the rotational velocity and rotational acceleration of the tool is limited by the joint velocities and accelerations. Table 2.3 shows the fullload joint velocities and accelerations of the manipulator of Figure 2.2. The waist rotation 01 has the smallest fullload acceleration since for most moves, the loading on joint 1 is the maximum. We observe from Table 2.3 that the fullload velocities and accelerations of the last three rotations, 04, 05 and 06, are approximately twice as high as the velocities and accelerations of the first three rotations, 6l, 02 and 03* This is
typical of articulated robots with a spherical wrist. In this section, we decouple the manipulator degrees of freedom at the wrist into
positional and rotational degrees of freedom. The decoupling of the translation and rotation at the wrist yields a less conservative estimate of the rotational acceleration as compared to the case where the decoupling is not possible, since the rotational velocity and
acceleration constraints are determined independently for the first 3 and the last 3 degrees of freedom.
Decoupling rotational motion at the "wrist" joint
For most 6 degrees of freedom robot manipulators, the wrist joint (Figure 3.3) can be regarded as the decoupling joint between 3 positioning degrees of freedom and the 3 orientation degrees of freedom. The first 3 joints are called the positioning joints and the last 3 joints the orientation joints. The decoupling at the wrist has been employed to simplify the computations in the reverse (or world to joint) transformation, and the inverse dynamics for the robot [Hollerbach and Sahar 1983]. We define the 3 orientation degrees of freedom as the basis for a tool "rotation space" for a robot with a spherical wrist. The rotation space is created by the joint angles 04, 05 and 06 of Figure 3.3. A coordinate frame defined at the wrist joint is the reference frame for the rotation space. Note that the wrist frame changes between successive robot poses. But since the wrist frame is not a function of
the orientation angles, and can be determined independently from the tool pose, the rotation space angles 049 e5 and 06 are not changed. We determine the velocity and acceleration constraints in rotation space
and separate rotational motion planning into tool rotation in rotation space and the wrist rotation about the base frame. Since the first 3
joints are basically the positioning joints, we can anticipate most of the tool vector rotational motion planning in the rotation space.
In order to demonstrate the decoupling at a spherical wrist, we consider the manipulator of Figure 3.3 as an example. The "A" matrices
[Denavit and Hartenburg 1955, Paul 1981] for this manipulator can be derived as
c 1 0 sI c 1 2
A1 = s 0 c S 12 (3.21)
0 1 0 11
0 0 0 C
C 2 S 2 0 C2 1 3
A2 = 2 c2 0 s2 1 (3..22)
0 0 1 0 0 0 0 1
c3 0 s3 c3 1 4
A3 = 3 0 c3 s3 14 (3.23)
0 3 0 0 0 0 0 1
c 4 0 s 4 0
A4= [ 4 0 4 0 (3.24)
0 1 0 1 5
0 0 0
c5 s5 0 0
A 5 = s5 c 5 0 0 (3.25)
0 0 1 0 0 0 0 1
C 6 s6 0 0
A6 = s6 C6 0 0 (3.26)
0 0 1 1 6 0 0 0 1I
We will show that the velocity of the tool referred to the base coordinate frame can be decoupled into (1) a rotational velocity of the
tool vector with respect to the forearm (between elbow and wrist) and
(2) a velocity (rotational and linear) of the wrist referred to the base
frame. [Featherstone 1983, Hollerbach 1983]. It is usual for a robot motion specification to contain the linear velocity of the tool and the rotational velocity of the tool frame expressed in the fixed base frame. Let Bv t and B Wt denote the linear and rotational velocities of the tool vector.
From Fig. 3.3,
BP B B (3.27) Bw t ï¿½t Differentiating Eqn. (3.27)
B * B " B"
Iw Pt I~ (3.28)
BJ2!t = B[T6 . z
B . =B B[T6]
t= _ tx fT]z (3.29)
where [T6] = A1.A2...A6 and z = [0 0 ]T.
B * 6 Pt r
Figure 3.3
Pw
Decoupling end effector velocity at the wrist joint
kI 0
Substituting Eqn. (3.29) in Eqn. (3.28)
Bw t = B T6  (3.30)
 p  x [ t] z
Since P t t
B B B B
Pw V  w t x [T6] z (3.31)
Eqn. (3.31) gives the velocity of the wrist. We now determine the velocity of the tool with respect to the "wrist frame". The wrist frame
B[W] is defined by the product of the A matrices, Al, A2, A3 and A4. The rotational velocity of the tool frame is the vector sum of the wrist rotational velocity and the tool rotational velocity with respect to the wrist, all velocities referenced to the base frame.
B = _ + BW (3.32) t 3 t
weeB W
where B t is the rotational velocity of the tool from the wrist and B 3 is the wrist rotational velocity (due to 61, 02 and 83). Multiplying Eqn. (3.32) by BRw, we transform all velocities in Eqn. (3.32) to the wrist frame.
W W W W (3.33) ï¿½ = _ 3 3t
WW W W Ww (3.34) mt = t  m3
(AI" A2.   3 (3.35) 03 A '3) t 3
W " 3 is determined as follows:
B * B B (3.36) PW = 3 x W
Substituting for BP from (3.31) and, Bp is known from the joint coordinates, we can solve for the 3 elements of 13 in Eqn. (3.36). Then W 3 is given as
W) 3 = (A1 A2 A3)T B 3 (3.37)
Therefore, we can now determine the tool rotational velocity with respect to the wrist frame.
B
Thus, the tool frame rotational velocity, Bo t, can be divided into 2 components. One is the tool rotational velocity referred to the
B W
wrist frame, w , also called the velocity in rotation space and the other is the wrist rotational velocity, B referred to the base
_3 3' reerdt tebs
frame. Since the 2 rotational velocity components are independent, constraints on the rotational velocity of the tool frame can be transformed into constraints on the wrist rotational velocity and the tool rotational velocity from the wrist. Rotational Acceleration of the Tool Vector in Rotation Space
The magnitude of the tool rotational acceleration in rotation space is limited by the fullload velocities and accelerations of the 3
degrees of freedom in rotation space. In this section, we derive the

PAGE 1
COOPERATION AND COORDINATION OF COMPUTER CONTROLLED MANIPULATORS By SUJEET CHAND A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY UNIVERSITY OF FLORIDA 1984
PAGE 2
Dedicated to the memory of my father J
PAGE 3
ACKNOWLEDGEMENTS In my five years at the University of Florida, no one person has had a more profound influence on me than Dr. Keith Doty, my principal advisor and the chairman of my supervisory committee. I gratefully acknowledge the numerous hours of his time in extremely stimulating discussions that surpassed the common barriers of working and leisure times. His constant stress on perfection and a lucid writing style is, I .hope, reflected in this dissertation. I am extremely thankful to Dr. Basile, Dr. Bullock, Dr. Dankel , Or. Duffy and students at CIMAR for helpful discussions that contributed to this dissertation. I thank Dr. Ahmad Sadre and Bill Cartwright of General Electric Co. for the most pleasurable interface with the workings of the real world. To my many friends and colleagues in Gainesville, I owe a special thank you for all the good times. Of course, this work would not exist today if it were not for the constant support of my parents and sisters, to whom I am forever grateful . I acknowledge the financial support of the Department of Electrical Engineering during the years 198182 and 198283, and General Electric Co. for support as a Ph.D. Intern during the year 198384. 1 1 1
PAGE 4
* TABLE OF CONTENTS PAGE ACKNOWLEDGEMENTS iii LIST OF TABLES Vl LIST OF FIGURES VTi ABSTRACT jx CHAPTER I INTRODUCTION 1 II ONLINE POLYNOMIAL TRAJECTORIES FOR ROBOT MANIPULATORS 9 Constrained Joint Motion ~ An Improved Technique 10 Constrained Motion of EndEffector ~ Polynomial Approximation of Joint Trajectories 20 Effect of Error in Estimation of an Intermediate Velocity on Preceding Velocities in a Cubic Splines Trajectory 30 Application to Cubic Splines with Equidistant Knots 38 OnLine Cubic Splines for Joint Trajectory Interpolation...'.' 42 Scheme 1 42 Scheme 2 !.'!.*!! 46 Acceleration Jump in OnLine Cubic Splines..'!!!.'!.*!.'!!."]!."] 55 Results of Simulation and Experimentation !!! 60 III ROTATIONAL SPACE CURVES FOR CONSTRUCTION AND COMPLETE DESCRIPTION OF ROBOT TOOL ORIENTATION TRAJECTORIES 64 End Effector Motion Specification and Trajectory Planning... 65 Definition of A Rotation Space 69 Specification of Constraints on a Rotational Trajectory!!!!! 70 Explicit Constraints on the Rotational Trajectory 71 Implicit Constraints on the Rotational Trajectory 72 Construction of Rotational Trajectories Â— Conventional Translation Dominant Approach 74 Construction of Independent Rotational Motions !!!!!!!! 83 Planning Constrained Rotation Transitions Estimation of Acceleration for Spherical Wrist Manipulators g4 Decoupling rotational motion at the wrist joint!!! 85 Rotational Acceleration of the Tool Vector in Rotation Space 90 iv
PAGE 5
PAGE Point to Point Unconstrained Transitions , 101 Independent Rotations ~ An Illustrative Example 104 A Note on Wrist Rotational Velocity in Base Frame 114 IV TRAJECTORY SPECIFICATION AND LOAD DISTRIBUTION FOR CLOSEDLOOP MULTIMANIPULATOR SYSTEMS 116 Planning a MultiManipulator Task 119 Task and Constraint Description 121 Trajectory Specification for a MultiManipulator Task 122 Forces and Torques on an Effector in a Closed MultiManipulator Chain 123 Classification of Forces on an Effector in a ClosedLoop 124 Derivation of the Equations for the Force and Moment at the Object Center of Mass 124 Factors in a Force Distribution Scheme 128 Definition of "Force" Frames 132 Derivation of Force Frames from the End Effector Trajectories 133 Algorithms for ForceMoment Distribution 138 Definition of Average Incremental Work per Unit Force and Moment 139 A Decoupled ForceMoment Balance 141 Distribution of Moments ' 147 A General ForceMoment Distribution Scheme .' 149 Force and Mement Equality Equations 150 Inequality Constraints on Normal Forces and Joint Torques 151 Formulation of the Objective Function 152 An Illustrative Example ' 153 V CONCLUSION 163 REFERENCES 166 BIOGRAPHICAL SKETCH 170 V i
PAGE 6
LIST OF TABLES PAGE TABLE 2.1 Start point, stop point, and move velocity 21 2.2 Acceleration factors 22 2.3 Maximum joint velocities and maximum joint accelerations for the robot of Figure 2.2 23 2.4 Memory Error Factor for cubic splines with equidistant knots (n = 8, Av^= u^AVg) 41 3.1 Move parameters for Figure 3.2 80 vi
PAGE 7
LIST OF FIGURES FIGURE 2.1 A sample joint trajectory 13 2.2 Kinematic structure of robot used In simulation 24 2.3 Linear interpolation of joint trajectory 27 2.4 Sample points on a joint trajectory 31 2.5 Comparison of offline and online cubic splines. Velocity estimation by linear interpolation (Eqn. (2.49)) 47 2.6 Comparison of offline and online cubic splines. Velocity estimation by 1 point overlap, 2 point lookahead... 52 2.7 Comparison of offline and online cubic splines. Velocity estimation by 1 point overlap, 4 point lookahead... 53 2.8 Maximum path deviation vs. path velocity and update rates for linear and cubic splines interpolation of a typical Cartesian move 61 2.9 Online cubic splines showing no overshoot for an almost linear joint trajectory 63 3.1 A segment to segment transition 75 3.2 Rotational error vs. flyby time 81 3.3 Decoupling end effector velocity at the wrist joint 88 3.4 A simulation move 105 3.5 Trajectory of joint 1 vs. time for a Cartesian move of the end effector 107 3.6 Trajectory of joint 2 vs. time for a Cartesian move of the end effector.... lOs 3.7 Trajectory of joint 3 vs. time for a Cartesian move of the end effector IO9 VI 1
PAGE 8
PAGE 3.8 Trajectory of joint 4 vs. time for a Cartesian move of the end effector 110 3.9 Trajectory of joint 5 vs. time for a Cartesian move of the end effector Ill 3.10 Trajectory of joint 6 vs. time for a Cartesian move of the end effector 112 4.1 Translation and rotation of an object about the base frame... 126 4.2 3link planar manipulator used for simulation 130 4.3 Torque on joint 1 for the same Cartesian move but different startup configuration 131 4.4 Two manipulator move for simulation 155 4.5 Torque of joint 1 vs. time for the simulation move of Figure 4.4 157 4.6 Torque of joint 2 vs. time for the simulation move of Figure 4.4 158 4.7 Torque of joint 3 vs; time for the simulation move of Figure 4.4 159 4.8 Comparison of the sum of magnitude of joint torques for an equal load distribution and load distribution employing AIW factors I60 4.9 Plot of AIW^ vs. time for the simulation move of Figure 4. 4.. 162 V i i i
PAGE 9
Abstract of Dissertation Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy COOPERATION AND COORDINATION OF COMPUTER CONTROLLED MANIPULATORS By Sujeet Chand December 1984 Chairman: Dr. Keith L. Doty Major Department: Electrical Engineering This dissertation develops new techniques for robot motion planning and force coordination in multimanipulator tasks. We develop a new model for joint interpolated motion. A constraint on the trajectories of the faster, nonpacing joints clips both velocities and accelerations of the nonpacing joints without affecting the total move time. Experimental and simulation tests on the joint motion model clearly show the improved performance in controlled and smooth startups and setdowns of the manipulator joints and the end effector. This dissertation develops the first complete formulation for online tracking of an end effector trajectory by piecewise cubic spline joint trajectories. A factor called the system memory error is defined to estimate an ideal number of lookahead points on the end effector trajectory for a prescribed level of performance in the joint trajectories. Two methods are described for estimation of an additional constraint at the end points of each computational block of the online joint trajectory generator. Excellent results were obtained in ix
PAGE 10
simulation and experimental tests of the online splines for accuracy in tracking and smoothness of motion for a large number of constrained end effector motions. A new dimension to robot motion planning is added by the introduction, definition and development of independent rotational motion planning for constrained and unconstrained rotations of the robot end effector. We define a rotation space at the wrist joint of spherical wrist manipulators and prove that the maximum conf i guratonf ree tool rotational acceleration in the decoupled rotation space is the fullload acceleration of the slowest joint in rotation space. A simulation example with independent rotational planning illustrates the elegance of this approach for planning "feasible" and accurate rotational motions. We develop two linear models for "quasi static" load distribution in the coordination of a closedchain multimanipulator task. The models employ moving coordinate frames on the object as reference frames for load distribution. The moving frames, called the force frames, are defined from the trajectory space curves of the object and the end effectors. We show that the constraint specification of the load distribution problem is simplified by the computation of all constraints in terms of the force frames. The load distribution is set up as a linear programming problem with constraints on the maximum surface normal forces and the maximum joint torques. The objective function minimizes the system power and attempts a load distribution in the ratio of the individual manipulator payloads. X
PAGE 11
CHAPTER I INTRODUCTION The recent years have seen an explosive growth in factory automation. Increasing demands on productivity, work in hostile environments and mundane and repetitive tasks call for immediate application of robotic technology. As demands on the robot's performance increase, the level of sophistication of robotic technology must also increase. The articulated robot manipulator, a design motivated by the dexterous human arm, is commonly applied in many industries for tasks ranging from assembly and part sorting to welding and heavy material handling. The productivity in a task involving several subtasks can be considerably increased if multiple manipulators work cooperatively on the subtasks. It is easy to conceive of a large number of tasks that can be made faster and more efficient with coordinated movements of multiple robots working cooperatively. A few multirobot systems available commerically provide restricted interaction between the multiple robots in the system. Olivetti was the first to market an industrial robot system with multiarm option as a special feature [Bedina 1977]. The Olivetti SIGMA system can be configured with two or three arms, the only restriction being that the total number of controlled degrees of freedom (motors) cannot exceed eight. A limited amount of cooperation is allowed between the robots. Although the arms work in the same workspace, the subtasks for each arm are independent. Collisions are avoided by stopping one arm until the 1 J
PAGE 12
2 other moves out of the way. Thus, the Olivetti system is meant for tasks with independent subtasks, since direct interaction between the multiple robots is not allowed. In Japan, the Japanese government research institute has developed a prototype for an industrial robot called "Melarm" [Nakano et al . 1974]. Melarm has two arms, each with 7 joints and a gripper. Both arms are equipped with force feedback on every joint and several tactile sensors. An experimental system for automatic assembly of vacuum cleaners using two 8 degrees of freedom robots has been developed by Hitachi Ltd. [Takeyasu 1977]. Each arm has 3 fingers and about 30 tactile sensors on its grippers. There is no doubt that industry is aware of the numerous applications of cooperating robots to perform tasks hitherto impossible (or inefficient) for a single robot working alone. Research in robotics is distributed in diverse interdisciplinary areas such as sensory preception (vision, tactile and force) [Gleason and Agin 1979, Inoue 1974, Raibert and Tanner 1982, Stute and Erne 1979], manipulator design [Warnecke and Shraft 1979], motion planning [Paul 1979, LozanoPerez 1981, Whitney 1972], transformations [Duffy 1981, Paul 1981], dynamics and servo control [Bejczy 1974, Golla et al . 1981, Hollerbach 1979, Luh et al . 1980b] and locomotion [Giralt et al . 1979, McGhee and Orin 1976, Taguchi et al . 1976]. This dissertation attempts to provide the sophistication in the techniques for robot motion planning and force coordination needed to provide multiple robots the capability to perform tasks requiring a high degree of interaction between the robots. Most of the techniques developed in this dissertation are directly applicable to a single robot controller for better path tracking in end effector controlled motions, smoother start
PAGE 13
3 up and setdown in joint interpolated motion and planning independent rotational motions to make a rotation dominant trajectory "feasible" with smaller rotational errors. In Chapter II, we reformulate the trajectory generation problem, both for joint interpolated motion and end effector controlled motion. The reformulation is either an enhancement of the existing techniques or a new solution to the problem. At the joint level, we formulate the joint interpolated motion by a model that allows the startup and setdown accelerations of each joint to be controlled. We introduce a new constraint on the motion of the nonpacing joints that clips the startup and setdown acceleration as well as the velocity of the nonpacing joints, without affecting the total move time. The performance of the proposed model was tested on a 6degrees of freedom industrial robot. The results clearly show the superiority of the proposed model to the existing model [Anderson and Paul 1979, Paul 1981, Whitney 1972]. A new solution is derived for the online joint trajectory approximation [Paul 1979] during a constrained end effector motion in Cartesian space. The online joint trajectories are constructed from the current point and a few lookahead points on the end effector trajectory. Most robot controllers employ a 2point lookahead on the end effector trajectory in order to linearly interpolate the joint coordinates between the current point and the two lookahead points, and provide a quadratic smoothing from one segment to another if a discontinuity in velocity is detected between the two linear segments. A linearquadratic interpolation of the joint trajectory with a 2point lookahead [Paul 1979] produces considerable tracking error with higher
PAGE 14
4 velocities and lower sample rates on the end effector trajectory. In order to produce a better approximation of the joint trajectories with a limited point lookahead, we develop a scheme for online approximation of the joint trajectories by piecewise cubic splines. We formalize the estimation of the ideal number of lookahead points on the end effector trajectory in order to produce an accurate approximation of the joint trajectories by piecewise cubic splines. We define a factor called the system memory error, as a measure of the coupling errors in the estimation of the joint velocities in successive computational blocks on the online cubic splines trajectory. The ideal number of lookahead points is defined such that the coupling in the determination of the boudary conditions of successive computational blocks is lower than a specified threshold. The derivation shows that for a piecewise cubic splines approximation of the joint trajectories with equidistant knots, 4 lookahead points in each block produces a system memory error of less than 0.5 percent. This result indicates that for an equidistant cubic splines approximation, a reasonably good trajectory approximation with minimal coupling between successive computational blocks (less than 0.5 percent) can be obtained with 4 lookahead points on the end effector trajectory. A derivation for the acceleration jumps on the online cubic splines for the joint trajectory approximation shows that the acceleration jump, to a first approximation, reduces, when successive computational blocks are overlapped for improving the velocity estimate at the block end points. The online cubic splines improve the tracking accuracy of the end effector by providing a better approximation of the joint trajectories. The improved tracking is illustrated by
PAGE 15
5 sumulation. The online cubic splines were implemented on an industrial robot controller and tested for a large number of constrained end effector moves with different velocities and sample update rates. The online spline approximation technique showed excellent results in terms of smooth motion and tracking accuracy, for sample rates as high as 200ms and end effector velocities of up to 1.5 m/sec. The current techniques for end effector motion planning for a single robot are "translation dominant" [Finkel 1976, Paul 1979, Taylor 1979]. In other words, the planning of an end effector motion is based on the constraints on the end effector linear velocity and acceleration. Thus, for a given move, the constraints on the tool translational motion are employed to determine the move time. This move time is also forced on the rotational trajectory. Independent rotational trajectories have been underplayed in robot motion since rotations are difficult to visualize, and the determination of the maximum constraints on the tool rotational motion is also difficult. Therefore, as long as the rotational tool motion is uniform, no constraints are imposed on the rotational trajectory. Since translation and rotation are independent motions, rotational planning should be decoupled from the translational move planning. In Chapter III, we introduce and develop the concept of independent rotation planning in the robot "rotation space." It is shown that for a translationdominant planning of a common move where the robot end effector transits from one linear move segment to another, the rotational error, as defined in Chapter III, is directly proportional to the translation move time, an unjustified dependence that is not necessary. Also, for several robot moves, the translation dominant approach may not yield a rotational trajectory that conforms to
PAGE 16
6 the maximum constraints of the manipulator joint velocities and accelerations. We employ a common geometry of industrial robots, namely, manipulators with spherical wrists, to derive a simple method for independent rotational planning. For a spherical wrist manipulator, we decouple translations and rotations at the wrist joint. The first 3 degrees of freedom, from the base to the wrist, are called the positioning degrees of freedom. The last 3 degrees of freedom are said to constitute the rotation space of the spherical wrist manipulator. This definition of rotation space simplifies the independent planning of the tool rotations. Since the manipulator is decoupled at the wrist joint, and we independently determine the rotation degrees of freedom and the translation degrees of freedom, the reverse solution for the manipulator is simplified, since we separately determine the positional and rotational joints. Thus, only a pseudo reverse solution is required to determine the positioning degrees of freedom when the rotational trajectory is preplanned in rotation space. The concept of a pseudoreverse solution with preplanned rotational motion is extremely powerful for producing fast and accurate end effector translational trajectories for the class of .mani pul ators with nonspheri.cal or offset wrists, that only permit a recursive reverse transformation. In order to plan rotational motions independently, we require an estimate of the maximum rotational acceleration of the tool at any point in the robot workspace. The decoupling of the translation and rotation ensures a less conservative estimate of the rotational acceleration as compared to the case where the decoupling is not possible, since the rotational velocity and the acceleration constraints are determined
PAGE 17
7 independently for the first 3 and the last 3 degrees of freedom. The last 3 joint angles are usually twice a fast as the first 3 joint angles and therefore yield a higher estimate of the tool rotational velocity and acceleration. We derive the maximium limit on a value of the tool rotational acceleration that can be employed any where in the usable robot workspace (except at the singularity and limit points) and show that this acceleration is the fullload acceleration of the slowest joint in the decoupled rotation space. We also describe an algorithm for unconstrained point to point rotational transitions employing the maximum limitations on the joint rates and accelerations for a "feasible" rotational trajectory. A simulation of independent rotational motion planning clearly illustrates the elegance of the approach for producing smooth and feasible rotational motions for both constrained and unconstrained tool rotations. In Chapters II and III, we developed new solutions for joint interpolated motion, accurate online approximation of joint space trajectories and the construction of independent rotational motions of the robot end effector. Coupled with the existing techniques for translational motion planning, we now have the capability to generate accurate and feasible joint trajectories and end effector motions for a given robot move. Let us consider the task of displacing a long or heavy object by one or more manipulators. Given the destination of the object and the desired trajectory of the object, we can generate the end effector and joint trajectories of each robot. In order to determine the torque for each joint motor for a given trajectory, we require an active distribution of the object load among the manipulator end effectors holding the object at each sample point on the trajectory. Dynamic load distribution in a closedloop multimanipulator task is
PAGE 18
8 underspecif ied [Orin and Oh 1981, Williams and Seireg 1979], Since theoretically infinite solutions for the load distribution are possible, in Chapter IV, we develop two linear models with an objective function for a load distribution that minimizes the overall system power. We formulate the load distribution in Chapter IV in moving coordinate frames at each end effector. A moving coordinate frame called the force frame is defined from the space curve of each end effector trajectory. The 3 axes of the force frame are the tangent, normal, and binormal of the end effector trajectory space curve. The underspecif ied load distribution is set us as a linear programming problem, with an objective function for minimization of the system power for unit forces and moments in each direction of the force frame axes. By eliminating a direct dependence of the objective function on individual joint constraints, the objective function can be initially evaluated as any nonlinear function of the joint parameters for unit forces and moments in the directions of the force frame axes. This nonlinearity does not affect the linearity of our model, since the objective function is made independent of the individual joint parameters. In order to verify the load distribution algorithm, we simulate a simple two manipulator closedloop task. The simulation illustrates that the load distribution algorithm of Chapter IV reduces the overall joint torques of the manipulators in a closedloop task. An "equal" load distribution is employed to provide a comparison of the overall joint torques for the load distribution scheme of Chapter IV. Chapter V summarizes the main ideas in this dissertation, and provides insight into future work in the cooperation and coordination of multi robot operation.
PAGE 19
CHAPTER II ONLINE POLYNOMIAL TRAJECTORIES FOR ROBOT MANIPULATORS A smooth, accurate and predictable trajectory of all robot joints is of utmost importance in a multirobot task. Smooth trajectories for the robot joints and consequently, the end effector, are a necessity for cooperating robots. Robot motions can be classified into two types. One type of motion is called the joint interpolated motion. The other type is controlled motions of the robot end effector. In a joint interpolated move, the robot is moved from one set of joint coordinates to another without any control over the end effector trajectory. We develop a model that employs a splined polynomial trajectory for the joint trajectories in a joint interpolated move and describe a technique for scaling both the velocity and acceleration of the faster joints in a joint to joint move. Traditionally, there are two approaches to planning the joint trajectories for controlled end effector motions. One is the offline approach where all the joint trajectories are planned prior to the move. The other approach is to determine the joint trajectories online at a certain sample rate during the actual motion. The planning of offline trajectories is generally not a time critical process. Therefore, several wellk.iown techniques for polynomial trajectory approximation are directly applicable, the complexity and the accuracy of approximation is determined by the task requirements. The online trajectory problem, however, has received little attention since the 9
PAGE 20
10 lack of sufficient number of constraints on the online trajectory and the time constraint for computation limits the degree of accuracy of polynomial approximation. In this chapter, we develop the constraints on the online trajectory in order to produce a cubic splined trajectory for each joint. We derive the ideal number of lookahead points for generating an online joint trajectory, based on an approximation error factor called the system memory error. An analysis of the acceleration jumps in the online cubic spline trajectories is also given. The application of these trajectories to robot motion has been studied both by simulation and actual experimentation on an industrial robot. Both studies indicate excellent results in terms of smoothness of motion and accuracy of tracking the actual trajectory. Coordinated Joint Motion Â— An Improved Technique This section describes joint interpolated motion employing a splined polynomial trajectory. The most frequent use of a joint interpolated move in an industrial environment is for fast transits of the robot arm between jobs. A joint interpolated move provides the fastest transition of the robot arm in its workspace. The other type of move, namely a controlled motion of the end effector, is slower due to the conservative upper limit on the end effector speed (a value that is determined from the joint parameters and is valid for the entire robot workspace). Therefore, a joint interpolated motion is employed for fast but unconstrained transitions oi the robot end effector between points. A well known technique for joint interpolated motion is to determine the slowest joint for the move followed by scaling the velocities of the remaining joints such that the move time of each joint
PAGE 21
11 is that of the slowest joint [Anderson and Paul 1979]. All joints are initially accelerated to their maximum accelerations until the desired joint velocity is achieved. The joints then cruise at the desired velocities until the end of the move, where the maximum deceleration is applied on each joint. Due to the fast initial startup of all the joints at their maximum accelerations, the end effector (whose trajectory is not a concern in joint interpolated motion) experiences a large discontinuity in motion at the start (and end) of the motion. If the effector has a delicate tool fixture, the user may be prompted to use tool controlled motion (and sacrifice speed) for intermediate transitions. In order to reduce the initial discontinuity in end effector motion, we model a joint trajectory such that we start each joint motion by a cubic polynomial. The slope of the acceleration can be defined by the user as the slowness for startup. After a slow startup, the end effector motion is smooth, since all the joint trajectories are continuous. We define a general model for joint interpolated motions employing a cubic startup with a specified degree of "slowness." If the speed of the transition is of higher concern than the smoothness in startup and setdown, the cubic polynomial can be simplified to a quadratic. The new idea presented in this section is to coordinate the motion of all joints during startup, constant velocity and setdown segments. Thus, instead of coordinating the joint motion between start and stop points, the joints are required to be on the startup segment, constant velocity or the setdown segment for the same periods of time, determined by a pacing joint. It is shown that by assuming the same startup time for all the joints, initial jerk on some of the nonpacing
PAGE 22
12 joints is reduced, making the startup (and setdown) motion smoother. If the joint motion has to pass through an intermediate point then we could either use a cubic spline to pass through the point, or a quadratic polynomial as in [Paul 1979, Taylor 1979] for bypassing the point. A cubic spline is likely to produce an overshoot at the intermediate point, which may be unacceptable if the joint is close to a limit at the intermediate value. Therefore, a quadratic flyby is preferable if overshoot is not desired. Once again, all joints are on the flyby path for the same period of time determined by a pacing joint for flyby. By forcing all joints to be on the flyby path for the same period of time, acceleration requirement of the nonpacing joints is reduced. We now develop a model for the joint trajectories for a pointtopoint joint interpolated motion. In Figure 2.1, let J^ and J2 be the start and stop points for a pointtopoint move of a joint. The joint trajectory is divided into 3 segments called the startup, cruise, and setdown segments. A cubic polynomial is employed for the startup and setdown segments in order to vary the speed of startup and setdown. Assuming the joint starts from rest, the joint acceleration increases linearly from 0 to the required acceleration for startup. The acceleration profile of the joint is made up of 3 segments. For an initial period t^^.^., the joint accelerates linearly to the maximum servo acceleration. The maximum acceleration remains constant for a period t^Q^^g^ and then drops linearly to zero in another t^^^ seconds, such that the desired velocity of the joint is achieved. The joint acceleration for the startup time tg can be modeled as follows.
PAGE 23
13 Figure 2.1 A sample joint trajectory
PAGE 24
14 0 < t < t acc a(t) =a t 2tg^^ , is equivalent to v^ > a^^^^/m , where v^ is the desired velocity of the joint. In order to produce the fastest transition during the cruise segment, we employ a polynomial of degree one for the joint trajectory between the startup and the setdown segments. The motions of all joints are coordinated by the determination of the joint that takes the most time for the pointtopoint move. This joint is termed the pacing joint. The pacing joint time scales the move parameters of the nonpacing joints. Integrating Eqn. (2.1), Vq + 0.5 mt 0 < t < t. acc v(t) = v(t ) + a (t t ) ^ acc' max^ acc' Sec < ^ < (^cc " Sonst)
PAGE 25
15 v(t + t ^) + a (t t t J acc const' max^ acc const' 0.5 m (t t t J' acc const' (t + t ^) < t < t ^ acc const' a (2.2) where Vq is the initial or start velocity, v(tg^j.) is the velocity after tacc seconds. vCt^^^ + t^^^^^) is the velocity after (t^^c + t.^.^^) seconds. Integrating Eqn. (2.2) to get the equation for the joint displacement, and assuming v^ = 0 (joint starts from rest). 0(t) = J(0) + mt /6 0 < t < t acc J(t,,,) + v(t^^^) (t t_) + 0.5 a (t t )' acc' acc' ^ acc' max ^ acc' t < t < (t + t ^) acc ^ acc const' 'acc const' ^max ^acc " ^const^'/^ m (t t^^^ t^^^^^)3/6 (^acc ' ^const) i * 1 ^ (2.3) Let denote the specified velocity of a joint for the pointtopoint move. This velocity is usually given as a percentage of the
PAGE 26
16 maximum joint velocity. The startup and setdown time t^ can be determined from Eqn. (2.2) as ta = V ./a + a /m d max max (2v^/m) f/2 t > 2t a acc t = 2t a acc (2.4) Let tgj^ denote the time for the cruise segment. If Ae is the joint displacement between the 2 points of the pointtopoint move, then A9 = (Ae)2, + v^ . t^^ (2.5) *ab = (^/^d^ (^^)2t ^ (2.6) a where (^9)2t denotes the joint displacement in the startup and seta down segments. We can derive (^9). from Eqn. (2.3) and Figure 2.1 "a as a ^max 4nst ' ^(^acc ' ^const^ ' ^acc^ (^.7) In Eqn. (2.7), ^acc = ^max/"^ (2.8) ^const ~ (^a '2 t^^^) (2.9)
PAGE 27
17 ^(^acc) = W ^cc (2.10) Thus, from Eqns. (2.4) and (2.6), the move time for each joint can be determined as (2tg + t^i^). The joint that gives the maximum move time is the pacing joint. Let T^denote the move time for the pacing joint and the pointtopoint move. In order to coordinate the motion of all the joints, the move parameters for the nonpacing joints must be determined based on the total move time, T^ . An accepted and the most frequently used method for determination of the move parameters of the nonpacing joints is to determine the velocities of the nonpacing joints, assuming that each joint accelerates to its maximum acceleration (tg > 2Tg(,(.) during startup (and setdown) [Paul 1979]. From Eqn. (2.4), assuming t^ > 2*acc ^^^^ equations for tg = 21^^^ can be derived similarly), ^d = \ax ^max/"^) (2.12) Since a^^g^, m are known, we need to determine tg for each nonpacing joint. Expressing (^9)2^of Eqn. (2.7) in terms of m, tg and a^^g^, and substituting in Eqn. (2.5), we get (^^^npj = i ' (^x/'" ' ^max ) tg a^^^ T./m (2.13) (Aax) ^a ' (^x/'" ' W ^i) tg a2^;T./m (Ae)^^. = 0 (2.14)
PAGE 28
18 From Eqn. (2.14), we can solve for t^. We then evaluate v^j from Eqn. (2.12). Thus, the move parameters for the nonpacing joints can be determined from m, a^^g^, , and A9. The determination of t^ by Eqn. (2.14) and v^ by Eqn. (2.12) clips the velocities (from the user specified percentage) of the nonpacing joints, without changing m and a^a^* Since some of the nonpacing joints may have smaller displacements and are faster than the pacing joint, the startup and setdown of these joints can be made slower than the pacing joint. The slower startup of some of the nonpacing joints smooths the initial jump in the end effector. This may be significant for a delicate tool attachment at the end effector. We now develop a simple technique for the determination of a factor for the slower startup of the nonpacing joints. This factor is a smaller value of "m" for each nonpacing joint. Since we now introduce an additional variable "m" in the determination of the nonpacing joint parameters, we need an extra equation or constraints for a unique solution. We realize the extra information by imposing the constraint that the nonpacing joints accelerate, cruise and decelerate to a stop for the same periods of time as the pacing joint. In other words, we coordinate the motion of the joints during the startup segment, cruise segment and the setdown segment. This constraint improves the esthetics of the motion, and as shown later, reduces the initial jerk in the end effector. We note that for a given move, there may be one or more nonpacing joints that will not yield the desired displacement with the above constraint. We now derive the condition for coordinating the motion of a nonpacing joint during startup, cruise, and setdown segments of the pacing joint. Let
PAGE 29
19 m' denote the new startup slope for a nonpacing joint. From Eqn. (2.13), substituting m' for m, we can derive an equation for m' in terms Â°^ ^a* ^max' (Ae)^^. as = Ti)} / {(Ae),pj a^^^ t^ (T. t^)} (2.15) If m' is less than m, then the nonpacing joint motion can be coordinated with the motion of the pacing joint during the 3 segments with a slower startup and setdown. If m' is greater than m, then the nonpacing joint cannot be coordinated during startup, cruise, and setdown of the pacing joint. In this case, the move parameters for the nonpacing joint are determined from Eqns. (2.12) and (2.14). This case can occur when the maximum acceleration of the nonpacing servo is less than the maximum acceleration of the pacing servo, and the displacements of the two joints are comparable. Thus, by employing Eqn. (2.15), we can determine the move parameters of the nonpacing joints, which are considerably simplified when m' < m, since t^ and t^j^ are known from the pacing joint parameters. In the above paragraphs, we developed a model for pointtopoint joint moves with cubic polynomials for startup and setdown. For manipulators with heavily damped servos, t^^^ may be specified as 0 which makes the startup and setdown trajectories quadratic polynomials. For a quadratic startup, by imposing the constraint that the startup time of a nonpacing joint is the same as that of the pacing joint, we can reduce the initial jump in the acceleration of the nonpacing joints. If k.a^^g^ denotes the acceleration of a nonpacing joint, then k can be derived as
PAGE 30
20 k = (Ae) . / {a t (t + t . )} (2.16) Note that k = 1 for the pacing joint. If k < 1 for a nonpacing joint, then startup, cruise, and setdown times of the pacing joint can be employed for the nonpacing joint, with the nonpacing joint acceleration clipped to k.a^g^^, k < 1. If k > 1 for a nonpacing joint, then the joint trajectory is planned such that the startup and setdown times are determined independently of the pacing joint, and only the cruise velocity is clipped such that the move time is . In order to illustrate the acceleration clipping factor "k" of Eqn. (2.16), Table 2.1b shows the value of k for each joint for a number of pointtopoint joint moves of Table 2.1a. The manipulator of Figure 2.2 is used for simulation. The maximum joint velocities and the maximum joint accelerations for the manipulator of Figure 2.2 are shown in Table 2.2. Note that for all the moves, the acceleration can be clipped for most or all of the nonpacing joints. The clipping can be greater than 9ff/^ of the maximum joint acceleration for some nonpacing joints. In the following sections, we describe the online determination of joint trajectories for controlled motions of the robot end effector Cartesian space. Constrained Motion of the End Effector Â— Polynomial Approximation of Joint Trajectories In a constrained end effector motion, the trajectory of each joint must be controlled to result in the desired trajectory of the end effector. The use of polynomial functions for generating accurate and smooth joint trajectories is a well known technique [Finkel 1976, Lewis 1974, Luh et al . 1983, Paul 1972]. It has been shown that a large number of
PAGE 31
21 Table 2.1 Start Point, Stop Point, and Move Velocity MOVE # START POSITION (DEG.) (61,62,63, 64,65, eg) DESTINATION (DEG.) (Â®1'^2'^3'Â®4'^5'Â®6^ % VELOCITY 1 0,0,0,0,0,0 30,45,45,20,50,40 70 2 10,25,30,45,60,75 80,45,45,45,45,45 50 3 10,10,10,10,10,10 60,60,60,60,60,60 70 4 10,10,10,10,10,10 60,70,100,60,60,60 40 5 10,20,30,45,45,45 20,20,60,40,40,40 30 6 10,20,30,45,45,45 100,45,45,70,70,70 100 7 10,20,30,45,45,45 10,30,40,45,45,45 60 8 60,30,40,45,10,50 10,50,20,10,50,70 80 9 50,10,45,60,30,40 70,50,10,20,50,10 60 10 _45, 45, .45, 60, 60, 60 30,50,60,30,30,30 70
PAGE 32
22 TABLE 2.2 Acceleration Factors MOVE # MOVE ^acc TIME ^const (SECS.) k2 ^3 k4 ^5 1 0.183 2.321 2.686 0.716 1 0.882 0.199 0.464 0.375 2 0.165 4.456 4.787 1 0.266 0.176 0 0.083 0.168 3 .183 2.599 2.965 1.073 1 0.882 0.448 0.418 0.422 4 0.104 5.737 5.945 0.894 1 1.121 0.373 0.348 0.351 5 0.125 6.335 6.585 0.406 0.504 1 0.48 0.447 0.452 6 0.33 3.3 3.962 1 0.55 0.56 0.094 0.088 0.089 7 0.185 1.905 2.275 0 0.263 0.232 1.062 0.99 1 8 0.264 2.624 3.153 1 0.266 0.704 0.208 0.222 0.112 9 0.156 2.439 2.753 0.537 1 0.772 0.897 0.209 0.527 10 0.183 5.103 5.468 0.847 1 0.975 0.425 0.396 0.4
PAGE 33
23 Table 2.3 Maximum Joint Velocities and Maximum Joint Accelerations for the Robot of Figure 2.2 JOINT ANGLE MAXIMUM JOINT VELOCITY (rad/sec) MAXIMUM JOINT ACCL. (rad/sec^) 0.515 1.5575 02 0.4365 1.5725 ^3 0.7595 1.896 ^4 1.257 3.728 ^5 1.24 4.00 ^6 1.22 3.961
PAGE 34
24 Figure 2.2 Kinematic structure of robot used in simulation
PAGE 35
25 undesirable properties of high order polynomials can be overcome by using lower order spline functions [Finkel 1976]. If a trajectory has a large number of points, then a spline fit for the trajectory requires all spline coefficients to be computed before the trajectory is executed. This is due to the inherent nature of spline calculations [Ahlberg et al . 1967, DeBoor 1978], Hence, a spline trajectory with a large number of intermediate points has to be computed offline. Techniques for piecewise splines using Xsplines [Clenshaw and Negus 1978] have also appeared in literature [Lin and Chang 1983]. In the following sections, we investigate the ideal number of lookahead points for a piecewise cubic spline trajectory and describe two novel techniques for breaking the offline spline computations into smaller blocks for realtime trajectory calculations. An end effector move is executed by transforming the end effector position and orientation into joint coordinates at a large number of points on the end effector trajectory. If the move is to be in real time with online trajectory calculation, we lookahead a few points on the end effector trajectory and generate the joint trajectories. The complexity of the interpolation scheme used to approximate the joint trajectories directly depends on the time interval between successive joint coordinates, i.e., the sampling rate on the end effector trajectory. If the sampling rate is high, then a polynomial of degree 1 may be adequate. For slower sampling rates, linear interpolation of joint coordinates will produce jerky motion and may even cause instability [Paul 1981]. In order to demonstate a typical joint trajectory during a constrained end effector motion, a plot of joint displacement versus time for a joint angle of a 5degrees of freedom
PAGE 36
26 robot for one particular move is shown in Figure 2.3. Joint target points for the elbow rotation (angle 6^) were generated at equal time intervals for a Cartesian move of the end effector of a robot whose kinematic structure is shown in Figure 2.2. A polynomial of degree one (straight line) is used for interpolation between successive target points. This form of interpolation can be unsatisfactory for a smooth servo response because a linear function in displacement produces impulsive accelerations between constant velocity segments. A well known technique for smoothing a joint trajectory is to use polynomials of a degree higher than 1. A single polynomial fit through all the intermediate points of a joint trajectory results in an extremely high degree for the polynomial. For instance, if the joint trajectory has n intermediate points, the minimum degree of one polynomial that passes through all the points is n1 (Lagrange interpolation). Splining low order polynomials provides an elegant way of reducing the overall degree and also the computational burden of trajectory generation. The degree of the polynomial should be such that the overshoot and wandering are minimal [Finkel 1976, Lewis 1974]. Overshoot occurs when the value of the polynomial goes beyond the "band" of initial and final values. Overshoot becomes obvious for a quartic polynomial when the "band" between start and end point is small. Wandering is retrograde motion. Both overshoot and wandering are undesi rable. A number of papers in recent literature have described the application of spline functions to the generation of smooth robot joint trajectories [Luh et al . 1983]. Splined polynomials are used for pointtopoint joint motion and motion involving a large number of
PAGE 37
27
PAGE 38
28 intermediate joint positions. [Luh et al . 1983] describes the construction of cubic spline joint trajectories with optimum placement of the knots. The cubic splines are constructed offline with a knowledge of the end effector trajectory. A classification of schemes using splined polynomials for constructing a joint trajectory with a large number of knots is as follows: (1) 433Â—4 trajectory The first segment is a 4th order polynomial specifying the trajectory from initial to the "liftoff" position of the joint [Paul 1981]. The following polynomials are all cubics except for the last trajectory segment which is a quartic. Consider the joint displacement trajectory shown in Figure 2.3. At the initial and destination points, the position, velocity and acceleration are known. At each intermediate point, the position or joint displacement is known. To make the interpolation curve doubly continuous at all points, the constraint that acceleration be continuous at the intermediate points where 2 polynomials join is imposed. Using this constraint at the intermediate points, a set of equations are generated that determine the slope of the curve at the intermediate points where 2 polynomials are splined together [Ahlberg et al . 1967, DeBoor 1978]. A formulation of this type results in a 433Â—4 trajectory. (2) 33Â—3 trajectory This is a cubic spline fit for all the trajectory segments. The only difference from the 433Â—4 scheme is that the acceleration constraints at the start and destination points are ignored to yield all cubic polynomials.
PAGE 39
29 Inherent in the spline formulation is determination of the slopes of the interpolating polynomial at all intermediate points between start point and end point. Thus, if there are n intermediate points, it is necessary to solve a matrix equation of dimension nXn. The equation involves solution of a tradiagonal matrix, which is uniquely solvable by the LU decomposition scheme [Johnson 1977], A typical motion trajectory of the end effector might result in 50 or more intermediate points for a joint trajectory. Solution of a matrix equation of dimension 50x50 is impractical both in terms of computational time and storage requirements and is feasible only for an offline trajectory planning scheme. For many robot applications, an offline generated trajectory is unacceptable. A specific example is sensor based control of the robot end effector, where the trajectory cannot be preplanned. In this case, splines for the joint trajectories must be generated online. Two schemes employing cubic splines for approximating joint trajectories with limited point lookahead are derived in this chapter. Experimentation has shown that (1) the online cubic splines provide smooth motion of the joints (and end effector) for a wide range of sampling rates, and (2) the computations that determine the required spline coefficients are simple enough for real time implementation. Also, simulation has shown that for slower sampling rates, the splines track the end effector trajectory much better than a simple linear interpolation. The following sections of the chapter are organized as follows. The next section derives an "ideal" number of lookahead points for generating cubic spline trajectories online. We then develop techniques for estimating an additional constraint at the last point of
PAGE 40
30 a lookahead block. An analytical derivation of the acceleration jump in the online splines scheme is also given. Finally, the results of experimentation and simulation are described. Effect of Error in Estimation of an Intermediate Velocity on Preceding Velocities in a Cubic Splines Trajectory In order to compute splines for a joint trajectory online, it is necessary to look ahead a few points on the end effector trajectory. The current point and the lookahead points are referred to as a "block," the block size being determined by the number of points in the block. Ideally, the size of a block should be such that an error in estimation of an additional constraint at the last point of the block should be localized to the block itself. For most robot controllers, the block size is fixed by the computational bandwidth of the controller for the determination of the lookahead points on the end effector trajectory. This section describes a method for the estimation of an ideal block size, which can be implemented if the controller provides the flexibility for changing the block size. Figure 2.4 shows a segment of an example joint trajectory with the start point PI, and two intermediate points P2 and P3. The block size in Figure 2.4 is assumed to be 3 (2 point lookahead). Two cubic polynomials with acceleration continuity at the intermediate point P2 can be evaluated for the present point PI and the two lookahead points P2 and P3, if one additional constraint at P3 is known. At PI, the position and velocity are known, at P2 and P3 the only information available is the position and 63). By splining 2 polynomials with continuity in acceleration at P2, the velocity at P2 can be determined [DeBoor 1981]. Therefore, with 4 constraints between PI and P2, a cubic
PAGE 41
31 'r "1 2 X I >^ P4 Figure 2.4 Sample points on a joint trajectory
PAGE 42
32 polynomial for the trajectory P1P2 can be computed. The polynomial P2P3, however, will be a quadratic since only one boundary condition (position) is known at the point P3. If an additional constraint at the point P3 (velocity or slope of the curve) is estimated, then a cubic polynomial can be evaluated for the segment P2P3. This sections studies the effect of the velocity at the block end point (P3 in the example of Fig. 2.4) on the velocities at the preceding points. An all cubic splines (333) trajectory with n intermediate points yields a tridiagonal matrix equation for the determination of the slopes v^,Vp,...,Vp at the n intermediate points [DeBoor 1978, Luh et al. 1983].
PAGE 43
33 Â•ll ^12 0 0 0 0 0 i2i ^23 0 0 0 0 '32 ^33 0 0 0 0 0 v(l) v(2) v(3) 0 0 0 0 0 0 Â— Oa n,nl ^n,n v(n) (2.17)
PAGE 44
34 where the coefficients a^j and are ^i.i = 2 MTi + T^^i) ^i,il " Vl ^i,i+l " """i bi = (3 * Vi/Ti) * (0^ G^.i) + (3 * T^/Vi) * (e^.^i e^.) (2.18) with T^, being the ith and the (i+l)th time intervals, 9^ the ith joint position at time t^ and Vq and are the velocities at the start and destination points (see Ahlberg et al . 1967, Luh et al . 1983) for details on the derivation of Eqn. (2.17)). The matrix equation (Eqn. (2.17)) is derived by imposing continuity in acceleration at the n intermediate points [Ahlberg et al . 1967, DeBoor 1978]. This equation can be uniquely solved for y^,^^ recursively [Johnson 1977]. We will show that if Eqn. (2.17) is broken into a number of smaller segments for piecewise splines, then an error in estimation of the velocity at the end of each smaller computational block affects the slopes at only a few preceding points, beyond which the error, as defined later by Eqn. (2.26), becomes negligible. It will be shown that for a cubic splines trajectory with equidistant knots, an error in estimation of an intermediate velocity causes errors of significant magnitude (in decreasing order) only in the 4 preceding velocities.
PAGE 45
35 A recursive solution to (2.17) can be derived as follows: Define Yj = ajj+^/3j j = 1, n1 ^1 = an (2.19) Also define Zl = (bi Vq Ti)/3i then = Vl J = (2.20) where a^j denotes the element of the ith row and jth column of the nXn matrix on the left hand side of Eqn. (2.17) and bj denotes the jth element of the vector on the right hand side of Eqn. (2.17). We will now derive the intermediate velocities v^ ... v^ in terms of the last velocity, v^^, and other terms involving zj and y j . The equations so derived will demonstrate the effect of a change or error in the velocity v^^ on the intermediate velocities.
PAGE 46
36 = zi Yi Z2 + Ti Y2 Z3 ... + {!)" Yn Yp.i Yn.2 ... Yi (2.21) Now, if v^j becomes + Av^^ then the magnitude of the error in is given by Avj. as follows: = (^Vl) ^n Vl Â•Â•Â• ^1 (2.22) = (^Vl) (^n/^n) (Vl/Vl) Â•Â•Â• (Ti/3i) we now show that the product of the terms on the right hand side of Eqn. (2.22) becomes 0 in the limit as "n" approaches infinity. This is easily demonstrated by showing that 0 < (T^ZPj) < 1 for i = 1 to n.
PAGE 47
37 = 2(1 + yi^) (2.23) Since T2 and Tj are both positive, ^iHi is positive and greater than 1. From (2.23) and (2.19) (2.24) From (2.23) and (2.24) $2/^2 ""s also greater than 1. By induction. is also greater than 1. Therefore, the right hand side of Eqn. (2.22) progressively reduces in magnitude with increasing "n". From Eqn. (2.22) we define a memory error factor yj > 0 as a measure of the influence of a change in the slope at the (rHl)st knot on the slope at the jth knot (j < (rnl)). hence Av . = y . (Av , ) Since lim y. = 0, a practical threshold value of y^ can be defined for a given application such that for some finite j, the error 3^/1^ = 2 (1 + (T.^i/T.) * (1 0.5/(e._^/T._^))) (2.25) n KJ (2.26)
PAGE 48
38 in estimating the velocity at the (rHl)st knot has no practical effect on the jth knot. In this chapter, we assume the system memory error is negligible when p^ < e and e is less than 0.5%. Thus, given the sampling intervals, we can determine an "ideal" block size (n) such that the error due to estimation of the slope at the last point of the block is localized to the block itself namely < e, where e > 0 provides some prescribed level of performance in trajectory motion. The next section applies these results to a cubic splines trajectory with equidistant knots. Application to Cubic Splines with Equidistant Knots If the knots of a joint trajectory are equidistant in time, then the elements of the matrix in Eqn. (2.17) simplify as follows: ^i,i= ' ' il,i= ' i,i.f T (2.27) Substituting for a^j in the expressions for y j , 3j and Zj in Eqn, (2.19) we obtain Yj = T/3j j = nl,...,l 3j = 4T T2/3j.i 3i = 4T ^j = (^j ^jl)/^j
PAGE 49
39 j = nl,...,2 (2.28) Now can be computed from (2.28) as follows: = 4T, 32 = 3.75T, 33 = 3.73343T, 34 = 3.73218T, 35 = 3.73218T,...,3n.i = 3.73218T Note that 3p appears to converge to the value 3.73218T for n > 4. We now show analytically that 3^ always converges to the value (2 + /3)T for a cubic splines interpolation with equidistant knots. Consider where 3* = (2 + /3)T (2.30) B2 = (2 /3)T (2.31)
PAGE 50
40 From Eqn. (2.29), it is seen that the B sequence is monotone decreasing for B^_^ > or 3^_^ < B^ Â• Since B^ = B^ (Eqns. (2.28) and (2.30)), the B sequence is monotone decreasing, and ^ = = (2 + /3)T (2.32) Since y= T/B. , J J = T/(2 + /5)T = 2/3 (2.33) From Eqn. (2.26), it is seen that the effect of a change in the velocity of the last point (^v^^) Â°" velocity of the i th point (v^) is succintly described by y. = (Av./Av^^), a value only dependent upon i < n. An example trajectory with 10 points is used to demonstrate the influence of a change in the last velocity on the preceding velocities. Initially, it is assumed that the position and slope of the trajectory at the start and final points are known. At the eight intermediate locations, only the position coordinate is known. Table 2.3 shows the changes in the velocities of the eight intermediate points due to a change Avg in the last (tenth) velocity. The table shows that a change in the velocity of any point on the cubic spline trajectory results in significant changes only in the preceding 4 velocities, beyond which, the effect of the change becomes insignificant, less than 0.5% of the change.
PAGE 51
41 TABLE 2.4 Memory error factor for cubic splines with equidistant knots (n=8, Av^= y^Avg) i Ui ^1 8 0.26794 7 0.07179 0 u .uiy 0.5%) for only the preceding 4 points. The slopes can be considered unaffected for all earlier points. The memory of the interpolation is said to be 4. This result can also be interpreted the following way. If the offline splines calculations are broken into blocks of 6 points each, and we estimate the velocity at every sixth point, then the error in estimation of the velocity is limited to errors in its block of 4 intermediate points.
PAGE 52
42 The next section describes schemes for estimating the slope of the trajectory at certain intermediate points for online spline computations with limited point lookahead. OnLine Cubic Splines for Joint Trajectory Interpolation Scheme 1 One way of determining an additional constraint at the last point of a lookahead block is to compute the joint velocities by an "inverse velocity" transformation. There are several approaches to an inverse velocity determination. Two general approaches are the use of an inverse Jacobean matrix [Whitney 1969], and Screw theory [Lipkin and Duffy 1982]. The data required for an inverse velocity transformation are end effector linear and rotational velocities and the configuration of the arm. The complexity of the inverse velocity transformation is dependent on the geometry of the manipulator. For many 6 degrees of freedom robots with intersecting axes at the wrist (spherical wrist), the inverse velocity computation can be simplified [Featherstone 1983, Hollerbach and Sahar 1983]. Since position and orientation can be decoupled at the wrist we can determine the velocities of the first 3 joints in one step and the velocities of the latter 3 joints in the next step. Since the reverse velocity determination is not computationally intensive for the class of manipulators with spherical wrists, it is possible to determine the splines for each lookahead block in realtime. With position and velocity constraints at the start and end points of a block, velocities at . each intermediate point of the block can be estimated by imposing continuity in acceleration and solving the tri
PAGE 53
43 diagonal matrix equation. If the block size is small, no matrix manipulations are involved since the equations for the intermediate velocities can be expressed in closed form. Thus, with position and velocity constraints at each intermediate point of the joint trajectory, and ignoring the acceleration at the start and final points, cubic polynomials can be fit for the entire trajectory. The first and last polynomials will be quartics if initial and final point acceleration is considered. The interpolating curve will be continuous in the first and second derivatives at all points except the block end points where continuity in second derivatives is lost. We now briefly outline the equations for generating the spline coefficients for each block. For simplicity, a 2point lookahead is assumed. A normalized time "u" for each segment of the joint trajectory is defined as follows: For the ith segment, let u = (t ti.i)/(Ti.i) where i1 = t. t. i1 (2.34) All polynomials are expressed with respect to u. du/dt = 1/T^_2 on time segment i (2.35) Let fi (u) = a3^ u^ + a2T u^ + a^i u + agj (2.36) For the ith segment, f^(u) is evaluated from u = 0 to u = 1.
PAGE 54
44 df^(u)/du = (dfi(u)/dt)/(du/dt) = T^.i * (dfi(u)/dt) (2.37) Also, d^ (f^(u))/du2 = t2._^ * (d2f.(u)/dt2) (2.38) Equations (2.37) and (2.38) transform the velocity and acceleration of the joint at t^ to velocity and acceleration of the normalized trajectory. Let f^(u) = ^ ^21 + u + agi = 0 (2.39) and f2(u) = + + 3^2 " ^22 = 0 (2.40) be the cubic polynomials describing the trajectory between and P2, and P2 and P3 in Figure 2.4. Let 9]^, 63 denote the velocities at P^ and P3 respectively. We know from the polynomial preceding P^. If P^ is the start point of the trajectory and the arm starts from rest, then 6^ = 0. We can determine 63 by a "reverse velocity" transformation at time instant t3 (point P3). Splining (2.39) and (2.40) together with continuity in acceleration at P2, we derive an expression for Â§2 as follows: (6a3j u + 2a2i)/T^  _ = {6a^^ u + 2a22)/T^ u = 1 (2.41) u = 0 Expressing all coefficients in (2.39) and (2.40) in terms of e^, Â®2Â» Â®3' ^2' ^3Â» 96t
PAGE 55
45 aoi = ^1 ' = eiTi (2.42) = 3 e^) (02 + 2ei)Ti (2.43) = 2 (^2 ^l) (Â®1 ^ ^2)^1 (2.44) ao2 = ^2 ' ai2 = ^2'r2 (2.45) 322 = 3 (63 62) (63 + 262)12 (2.46) 832 = 2 (63 62) + (Â§2 + 63)12 (2.47) Substituting in Eqn. (2.41) and solving for 62 02 = 1/2 (Ti + T2) [(3T2/Ti)(62 6^) + (3Ti/T2)(63 62) 61X2 6311] (2.48) The coefficients of the two cubic polynomials are computed from Eqns. (2.42) through (2.47). An advantage of using actual joint velocities as constraints at certain intermediate points is that each joint will closely track the actual velocity required of it to maintain a desired end effector velocity. This implies that the velocity profile of the end effector between intermediate points and at intermediate points will be closer to the desired value. As mentioned, acceleration continuity is lost at the block end points.
PAGE 56
46 For some manipulator geometries, an inverse velocity computation may be computationally too intensive for a limited point lookahead, online trajectory. A second formulation is proposed to estimate an additional constraint at the block end points. The computations are independent of manipulator geometry and are simple enough for online implementation. Scheme 2 The simplest approach to determination of an additional constraint at block terminal points is to compute joint velocity at that point by a linear interpolation. Thus, at the point P3, in Figure 2.4, the joint velocity can be estimated as P3' = (P3 P2) / T2 (2.49) Again, in this case, acceleration will be discontinuous at the block end points (including the start and final points). Since the estimation of the velocity by Eqn. (2.49) is local to the specific block, the tracking error will be larger. The "locality" of an estimation of this type is illustrated in Figure 2.5. Figure 2.5 shows a plot of a joint trajectory with 7 points. The 5 intermediate points are spaced equidistant in time. The joint trajectory is for joint angle 4 of the robot shown in Figure 2.2 for a linear end effector move. The dashed trajectory is obtained by computing the 6 cubic splines by the conventional "offline" scheme. The slopes at the 5 intermediate points for the offline scheme were determined by solving a 5X5 tri diagonal matrix. The solid curve is obtained by estimating velocities at every other point (2 point lookahead) by Eqn. (2.49). The plots demonstrate
PAGE 57
47 0) E a I. o. (/) o 'ro 4> <0 =) O o Q. S(U 4> C 'tÂ— 1 c io ta 0) D (C rÂ— (U C JQ 1 o 4M+> O (0 E HO +J (/) c 0) o to Â•rÂ— s(0 u Q. o E o > Â• CM
PAGE 58
48 the tracking error resulting due to a poor estimate of the slope (Eqn. (2.49)) at the intermediate points. A significant improvement in the estimate of the slope at the block end points is achieved by the following technique. The idea is to estimate the slope at the block end points by overlapping successive blocks. An initial estimate of the slope at the block end points is made by assuming the last polynomial of the block to be a quadratic. This slope is then improved by "overlapping" the next block with the current block. The overlap is made between a point following the block end point and the point preceding the block end point. The improved estimate of the slope at the block end point is obtained by splining 2 polynomials between the preceding point, block end point and the next lookahead point with continuity in acceleration at the block end point. Referring to Figure 2.4, the slope of the curve at point P2 (midpoint of the 3 point block) is initially estimated. Since the slope at P3 is not known, the slope at P2 is determined independently of the slope at P3. This is done by tying together a cubic polynomial between PI and P2 and a quadratic polynomial between P2 and P3 with continuity in acceleration at P2. Thus, by making acceleration continuous at P2, the slope of the curve at P2 is determined irrespective of the slope at P3. Now when point P4 is known, the slope at P3 is computed again using the same formulation as above. This new slope allows a cubic polynomial to be fit between P2 and P3 instead of a quadratic. Also, the new slope at P3 is an improvement over the one computed by differentiating the quadratic. Thus, an improvement in the estimation of the slope is obtained by overlapping successive blocks. Let
PAGE 59
49 (u) = 33^ + ^ + a^]^ u + = 0 (2.50) f2 (u) = ^ ^12 ^ ^02 ~ ^ (2.51) be the equations of 3 cubic polynomial between P2 snd 3 qu3dratic polynomi3l between P2 P3 (Figure 2.4). The norm3lized time V3riable is denoted by "u" as before. The initial conditions at P^, P2 and P3 are shown in Figure 2.4, except in this case, there is one less boundary condition at P3; th3t is, 63 is not known (recsll th3t in Scheme 1, 63 was initially determined by a "reverse velocity" computation). Hence, f2(u) is given by a quadratic equation (2.51). Splining (2.50) and (2.51) together with continuity in acceleration at P2, 82 is determined as follows: 6331U + 2321/T2' = 2322/T2' (2.52) u = 1 u = 0 Expressing 3ll coefficients in (2.50) snd (2.51) in terms of ^' ^1' Â®2' ^2' Â®3 am = 9 01^1 . an = 9iTi (2.53) 321 = 3 (62 e^) (201 + e2)Ti (2.54) 831 = 2 (92 ^l) + ih + e2)Ti (2.55) a02 = ^2 Â» ^12 " ^2^2 (2.56)
PAGE 60
50 = (^3 " ^2^2 (2.57) Substituting in Eqn. (2.52), we evaluate Â§2 as 62 = (T^T2/(2T2 + T^) * [(1/T^)(e3 62) + (3/T^)(02 6^ ) Q^/J^l (2.58) When point is known, is determined by splining a cubic polynomial between ?2 P3 and a quadratic between P3 P^. 63 = (1213/(2X3+ T2)) * C(i/T^)(e463) + (3/T^)(e3 62) 92/T2] (2.59) With the new value of 63 given by (2.59), coefficients of a cubic polynomial between P2 P3 are computed (instead of the original quadratic) as follows: ao2 = 02 ' ^12 = V2 (2.60) 322 = 3 (63 62) (202 + 63)12 (2.61) 832 = 2 (63 62) + (62 + 63)12 (2.62) Thus, the coefficients of the cubic polynomial between P^ P2 are determined from Eqns. (2.53), (2.54), and (2.55) with a knowledge of 6^, 6^, 62 and 63; and the coefficients of the cubic polynomial between Pp P3 are determined from Eqns. (2.60), (2.61), and (2.62) with a knowledge of 62, 62, 63, 83 and (point P4).
PAGE 61
51 The improved tracking accuracy of this scheme is demonstrated in Figure 2.6. The plots of Figure 2.6 show the same joint move as Figure 2.5. The dashed trajectory is the "offline" spline solution. The solid trajectory is obtained by a 2 point lookahead on the joint trajectory with slope determination as described above. In order to illustrate the improvement in trajectory with the number of lookahead points. Figure 2.7 shows a plot of the joint trajectory of Figures 2.5 and 2.6 with a 4point lookahead. In the preceding section, we derived that a lookahead of 4 points for an equidistant knots cubic splines trajectory produces a system memory error of less than 0.5 percent. We deduce from Figure 2.7 that the piecewise splines track the offline splines trajectory with almost no undershoot or overshoot, illustrating better tracking due to negligible coupling in the determination of the velocities at the block end points of successive computational blocks. In Eqn. (2.59), the slope at P3 was determined with the overlap in the succeeding block limited to a single lookahead point (P4). With this technique, joint trajectory interpolation can start with a 2point lookahead for the first block, after which, only 1 lookahead point on the trajectory is required to compute the next joint polynomial. Thus, this scheme is adaptable to sensorbased control where smaller lookahead means quicker adaptation to the new trajectory. The estimate of 9^ by Eqn. (2.59) can be further improved by employing both points of the next lookahead block (2point lookahead) to compute 6^ . In this case, we spline two cubic polynomials between P2 P3 and P3 P4 and a quadratic polynomial between P^ Pg, with continuity in the second derivative.
PAGE 62
52 o 1 u c c o Â•ro T3 C 1 (O (U >, 1 o M4+J O (O E MÂ•fÂ— O 4J to C o >> ^ SÂ•1to fO U 1 Q. o ^ E 1Â— o o a' o o > 1Â—
PAGE 63
53 (/) o +J c o o o D. c TO (U >. C X) 1 o <4Â•(J O E l(o M to c: 0) a o (/) >i ^ sÂ•r(0 (0 U 1 Q. o s*: B 1Â— o o O! o (_) > rÂ—
PAGE 64
54 The acceleration continuity equations at P3 and are (6a.Â„u+ 2aÂ„)/T^ , = (6a^Â„u + 2aÂ„)/T^ , (2.63) ^'^ ^ \ w = \ " u = 0 (6a33U + 2a23)/T^ = 2a24/T^ (2.64) Substituting for the coefficients a^j in Eqn. (2.63) and simplifying, we get 263 {l/T^) + (I/T3)} + yT3 = (3/T^) (e^ e^) yi^ (2.65) Substituting for the coefficients a^j in Eqn. (2.64) and simplifying (63/13) + 6^ {(2/T3) + I/T4)} = (1/T^) (65 64) f (3/T^) (64 63) (2.66) Solving for 63 and 9^ from Eqns. (2.65) and (2.66), 63 = [{T2T3 (T3 + 2T4)} / {2(T2 + T3) (T3 + 2T4) T3T4}] . [(3/T^) (0^ e^) {2J^n\ (13+ 2T4)} (64 63) {I/T4 (T3+ 2T4)} (65 e^) l^n^l (2.67) \ = {V4/(T3+ 2T4)} [(1/T^)(e5 e^) + (3/T^) (64 63) 63/13] (2.68)
PAGE 65
55 We note that the overlap correction causes a discontinuity in acceleration due to the improved velocity estimate at the block end point. For a 2point lookahead, and velocity estimations by Eqns. (2.58) and (2.59), acceleration is discontinuous at the block end point and the point preceding the block end point. If the velocity at the block end point is determined by Eqn. (2.67), then acceleration continuity is lost at the point preceding the block end point. In the next section, we show that the discontinuity in acceleration due to the online cubic splines employing overlap velocity correction is, to a first approximation, always smaller than the acceleration jumps in an online cubic splines without overlap velocity correction. Acceleration Jump in Online Cubic Splines The acceleration jump at the "join" point of two cubic polynomials, f^(u) and f^+i(u) is given as M. = [(6a3.u 2a2i)/T.2]^^^ [(6a3(.^^)U 2^2{nl)/l ,1^^ (2.69) where f. (u) = a3.u'^ + a2u^ + a^.u + a^. (2.70) . Vl(^) = ^3(i+l) ^2(i+l)^^ ^ ^l(i+l)^ ^ ^O(in) (2^1) For simplicity, we assume equal time intervals; that is, T^.^^ = T . Substituting for the cubic polynomial coefficients in terms of 6, 6 (as in Eqns. (2.42) (2.44)), in Eqn. (2.69), we obtain
PAGE 66
56 [Aa].^^ = (1/T^) [6 (e.^^ e.) + (2e. + 4e.^^)T 6 (^2 Vl) 2 (^2^ 2Vi)T] (2.72) [Aa].,l = ^ [3 (9.^2 ^ T(9. . 49.^^ . 9.^2^^ (2.73) Equation (2.73) gives the jump in acceleration at the point where two cubic polynomials are splined together. In order to estimate the acceleration jump in the online cubic splines approximation, we assume that the actual joint trajectory is continuous and nth order differentiable, where n could be as large as desired. This assumption is not unreasonable for continuous motion of the robot end effector since the end effector trajectory is a composition of the joint trajectories (applying the well known theorem, the composition of continuous functions is continuous and vice versa). The acceleration jump will be estimated in terms of the derivatives of the continuous nth order differentiable joint trajectory. We initially estimate the acceleration jump for an online splines joint trajectory with no velocity correction at the end points of each computational block. For a 2point lookahead (referring to Figure 2.4 again for the example trajectory), the velocity at the block end point (P3) is simply the derivative of the quadratic polynomial between P2 P3Let ^j.i' ^j Â» ^j+i 3nd 9^.^^, 9.^2Â» ^Â•,Â•+3 denote 2 successive computation blocks. We will estimate the jump in acceleration at the (i+l)st knot. The velocities 9. , and 9. Â„ are
PAGE 67
57 Vl = (6.^^ 6.) e. (2.74) The acceleration jump at the (i+l)st knot is [Aa].,^ = (2/t2) [. 3 (9.^2 ^) + T (9. + 49.^^ ^1.^^)1 = (2/1^) [3 (9.^2 9^) + T {9. + (8/T) (9.^^ . 9. ) 49. + ^(V3V2)/3T[(V2^ Vl)/T^n (2.76) Rearranging terms in Eqn. (2.76), [Aa].^l = (2/1^) [(1/3) [9.^3 9.) (7/3) (9.^2 " ^) ^ (19/3) (9.^^ 9.) (8/3) 9.] (2.77) Expanding the terms in Eqn. (2.77) by Taylor series about 9^ we get 9^ (3T) + 9}^ (3T)2/2 + 911 (3T)3/6 + 9^^^ (3T)^/24 + ... (2.78) 9J (2T) + (2T)2/2 + 9JII (2T)3/6 + gj^'^) (2T)^/24 + ... (2.79) (V3 ^) (V2 ^) =
PAGE 68
58 (e.^j e.) = ej (t) + ej^ t^/2 + ej^^ t^/3 + ej^^^ + ... (2.80) From Eqns. (2.78), (2.79), (2.80), and Eqn. (2.77) [Aa].^^ = (2/T^) [0.5555 T"^ ej^^ 0.16667 e^^^ 0(T^)] = [(1.111 T) ej^^ + (0.3334 T^) e^^) + 0(T^)] (2.81) We now estimate the acceleration jump due to velocity correction at the block end points by Eqn. (2.59) which assumes a 1point overlap. The velocities e^^^ and e^.^^ ^O"^ this case (again assuming a 2point lookahead) are given as e^+i = (1/3) [(e.^2 ^ + 3 (6.^^ e.)/T e.] (2.82) Vz = (1/3) [(6.^3 9.^2)/T 3 (e.^^ " Vl^/^ Vl^ ^''^^^ Substituting Eqns. (2.82) and (2.83) in Eqn. (2.73) and simplifying [Aa].^^ = (2/T^) [(0.^3 e.)/3 10 (e.^^ " + 13 (e.^j e.)/9 2T e./9] (2.84) Expanding the terms in Eqn. (2.84) by Taylor series as before and simplifying [Aa].^^ = (2/T^) [(0.2592 T^) + (0.444 T^) eP^^ + 0(T^)] (2.85)
PAGE 69
59 [Aa].^^ = (0.5185 T) + (0.8888 T^) ej^^^ + O(h^) (2.86) Finally, we estimate the acceleration jump for velocity correction by a 2point overlap as in Eqn. (2.67). Note that in this case, there is no acceleration jump at the block end point. The jump in acceleration is at the point preceding the block end point. Therefore, with the two blocks 6._^, e. , e.^^ and e.^^, e.^^' > the accelerations jump is at the ith knot. The velocities 9. and 6.^^ are e. = (1/T) [(e.^j 9.)/3+ (6. 0._^)] e._^/3 (2.87) e.^^ = (1/llT) [9.^3. 7 9.^2^ 2 9.^^ 9. . 3 9._^] . 9._^/ll (2.89) [Aa]. = (2/T^) [3 (9.^^ 9._^) + (9._j +49.+ 9.^^)] (2.90) [Aa]. = (2/T^) [(9.^3 9._^)/ll + 7 (9.^2 " ^il^/H " 49 (9.^^ 9._^)/33+ 5 (9. 9._^)/3 8 T 9._^/33] (2.91) [Aa]. = (0.3838 T) ^ (^^^^ Â° (T^) (2. 92) Thus, Eqns. (2.81), (2.86), and (2.92) give the equations for the acceleration jumps due to online splines employing no velocity correction, and velocity correction with a 1point and 2point overlap respectively at the block end point. To a first degree of approximation on T, the magnitude of the acceleration jumps due to the overlap schemes
PAGE 70
60 are always smaller than the acceleration jump due to online splines with no velocity correction at the block end point. As "T" increases, the second order terms show that the increase in the magnitude of the acceleration jump is more rapid in the splines with the overlap correction (Eqns. (2.86) and (2.92)). Results of Simulation and Experimentation In this section, the performance evaluation by simulation and experimentation of Schemes 1 and 2 of the previous section, is presented. Online cubic splines approximation of joint trajectories provides the servo with a smooth joint displacement and velocity. It also ensures better tracking of the end effector trajectory. The tracking accuracy of the splines is shown for a particular Cartesian move by the plots (Fig. 2.8) of the end effector path error in mils (1/1000 of an inch) versus path velocity for 3 different transform update rates. The transform update rate in milliseconds is the time interval between successive joint coordinates. Experimental plots for a variety of robot moves not involving singularities and limit points demonstrate characteristics similar to those in Figure 2.8. The length of the linear end effector move used in our example simulation was 1 meter. The average path deviation for the entire path for different velocities and transform update rates are plotted in Fig. 2.8 for this move. For comparison, Figure 2.8 shows the average path error for a path velocity and a transform update rate when a linear function is used for joint trajectory interpolation (as in Figure 2.3) and cubic splines of Scheme 2 are used for interpolating the same joint trajectory. From Figure
PAGE 71
61 path deviation I (mils) Â— Linear interpolation in joint space cubic splines with Â— 2 point lookahead 200 ; 200 ms .' update ' 150 ms update 150 100 Â• 50 20 40 60 200 ITS update 150 ms update 50 ms update 50 ms update path velocity (in. /sec) Figure 2.8 Maximum path deviation vs. path velocity and update rates for linear and cubic splines interpolation of a typical Cartesian move
PAGE 72
62 2.8, it is seen that for all transform update rates for this particular motion, the cubic splines track the end effector trajectory with greater accuracy than the linear interpolation of joint coordinates. The splines cause a small overshoot at points on the joint trajectory where a change in direction occurs. Experimentation with a large number of linear end effector moves has shown that the error in end effector trajectory due to a small overshoot in one or more joint angles is insignificant, unless operating close to a joint limit. Figure 2.9 shows a joint trajectory which is almost linear with time. Cubic splines of Scheme 2 with a transform update rate of 100 ms are used for interpolating the joint trajectory. Notice that the cubic splines track this trajectory with no overshoot. Both Schemes 1 and 2 were tested on a 6 degrees of freedom robot, the kinematic structure of which is shown in Figure 2.2. Experimentation with a range of transform update rates from 50ms to 200ms showed that the cubic splines produced smooth motion of the joints during a Cartesian motion of the end effector. Linear interpolation of joint coordinates showed visible jerkiness in motion for update rates higher than 100ms.
PAGE 73
63
PAGE 74
CHAPTER III ROTATIONAL SPACE CURVES FOR CONSTRUCTION AND COMPLETE DESCRIPTION OF ROBOT TOOL ORIENTATION TRAJECTORIES In Chapter II, we developed novel techniques for the construction of joint space trajectories for both joint interpolated motion and constrained tool motion of the robot manipulator. In order to accurately track the end effector path by the online approximation of the joint trajectories, the controlled end effector trajectory in cartesian space must conform to the "feasibility constraints" of the manipulator. Feasibility constraints on the tool motion are the maximum tool velocity and acceleration given by the physical bounds on the velocity and acceleration of the individual joints. Since the tool is translating as well as rotating about an axis in space, both translational and rotational motions of the tool must satisfy the feasibility constraints. The current approach to planning an end effector motion is "translationdominant" [Paul 1981, Taylor 1979]. A translation dominant approach is the planning of a trajectory based on the translational move parameters. The rotational trajectory is constructed from the move time of the translational trajectory. For those motions of the end effector where there is a greater rotation than translation, as in the rotation of the tool in the tool frame, translation dominant techniques may be deficient or even fail to produce a continuous and "feasible" rotational motion of the manipulator. Again, by "feasible" rotational motion, it is meant that the rate of change of the tool rotation and the acceleration of the change is 64
PAGE 75
65 limited by the maximum rate and acceleration constraints on the joints of the manipulator. Also, a translationdominant move makes the rotational error in moves such as those requiring an instantaneous turn of the robot tool, proportional to the translational move parameters. Since translation and rotation are independent motions, we introduce, develop, and describe the techniques for the independent determination of the tool rotational trajectories in this Chapter. End Effector Motion Specification and Trajectory Planning It is common in practice to specify the translational trajectory of a robot tool tip to the required degree of exactness for a given robot task in 3space. Since the tool, in general, also rotates along its translational space curve, and rotation and translation are independent motions, a complete specification of the rotational trajectory is in order. Emphasis on rotational trajectories has been underplayed in robot motion since it is difficult to visualize and specify an exact tool orientation trajectory. Therefore, as long as the rotation is uniform and smooth along the translational tool trajectory, no constraints are imposed on the specific nature of the orientation trajectory. The simplest tooltip trajectories in Cartesian space are either linear or a composition of linear paths. Along a point to point linear path, there are two wellknown methods of rotation interpolation. One approach is to determine an axis in space such that the tool rotates about this axis with constant velocity and the total rotation about this axis transforms the initial tool frame to the final tool frame [Taylor 1979]. A second method described in [Paul 1979] uses the freedom of
PAGE 76
66 choice of the rotational space curve to split the total rotation about the linear path into 2 independent rotations. One rotation aligns the tool vector from an initial to the final orientation, and the other rotation is about the tool axis itself, to align the faceplate vectors. The two rotations are interpolated linearly between the start and destination points. The intermediate orientations of the tool on the linear path for Paul's and Taylor's schemes are generally different. The nonexistence of a unique rotational trajectory illustrates the fact that the rotational motion of the tool tip on a cartesian path is underspecified. One of the objectives of this Chapter is to define additional constraints on the rotational space curve to resolve some ambiguity in interpolating the tool orientation along any Cartesian space curve of the robot tool. Two approaches are described for the specification of constraints on the rotational space curve. The first one is based on a description of the relationship between the rotation space curve and the translation space curve. The second approach is to completely specify the rotational curve in rotation space only. For most point to point moves, a uniform tool rotation is desirable due to smaller inertial forces on the tool. If the objective is to simply transform the tool smoothly from one orientation to another, as in a tool transiting between work points, or tracking an object based on sensory feedback, the only constraint on the tool motion is smoothness. We term this type of tool motion unconstrained. Another example of unconstrained tool rotation is during the transition of the tool translational path from one linear segment to another. The only constraints on the tool rotational trajectory in a motion involving a transition from one linear segment to another is the continuity in
PAGE 77
67 position and velocity at the 2 boundary points. Most of the existing techniques for a smooth unconstrained tool rotation employ a "translation dominant" approach for computing the time for the tool rotational transition. The translation dominant approach is to compute the move time based on the translational parameters (linear velocity and acceleration) of the tool. For some robot moves where the translational displacement is much smaller than the rotational displacement of the robot tool, a translation dominant approach may cause excessive acceleration demands on some joint servos of the manipulator. In this Chapter, we separate the translational and rotational motion by decoupling the manipulator degrees of freedom. A common wrist for many 6 degrees of freedom (DOF) industrial manipulators is the spherical wrist. In a spherical wrist, the 3 axes of the degrees of freedom from the wrist to the tool intersect at the wrist joint. For manipulators with spherical wrists, we derive a simple method for the independent determination of the rotational move parameters by decoupling the manipulator DOFs at the wrist into translational and rotational DOFs. There are some robot tasks that require constrained changes of the robot tool. For example, in an arc welding task, the orientation of the torch with respect to the work surface is always of primary concern. Thus, in case of an orientation change, the torch angle with respect to the work must be maintained constant. This type of orientation change is termed constrained. Motions with constrained tool orientation are executed by transforming the constraints into joint coordinates at every sample time instant. A simple method of determining the maximum constraints (velocity and acceleration) on the constrained rotational motion is given in later sections.
PAGE 78
68 In order to construct a continuous rotational space curve for an offline trajectory, a 3dimensional "rotation space" is defined. A rotation space is defined by rotations about three orthogonal axes of a fixed frame, such that a point in the frame represents a tool orientation. The order of rotations about the axes of the rotation space is fixed. Therefore, either the roll, pitch and yaw angles or the three Euler angles can be used for creating the rotation space with respect to the fixed frame. For most articulated 6degrees of freedom robot manipulators, the first three joints determine the wrist position and the last three joints the tool orientation from the wrist. There are different types of wrist joints in articulated manipulators. We can classify the wrist joints into 2 common types, an Euler wrist (spherical wrist) and an RPY (roll, pitch, yaw) wrist. The choice of the specific rotation space is based on the type of wrist of the manipulator. In the rotation space, well known curve fitting techniques can be employed for creating a smooth rotational space curve through a number of via points. Also, transition from one rotational space curve to another in rotation space can be conveniently smoothed to any degree by employing polynomial smoothing techniques. A continuous (at least in the first time derivative) rotation space curve not only gives a unique rotational trajectory for a move, but also guarantees a smooth transition from one velocity to another without having to stop at the point of transition. ' The next section describes the construction of rotation space.
PAGE 79
69 Definition of a Rotation Space A continuous rotation space can be created by choosing 3 independent rotations of the tool about a fixed base or universal frame. Thus, roll, pitch and yaw; or the 3 Euler angles can be employed to create the rotation space. The basis of the rotation space will be 3 orthonormal axes, each axis representing the roll, pitch or yaw, or the 3 Euler angles about the axes of a predefined base or universal frame. Any orientation of the robot tool maps to a point in the rotation space. The space curve of the points in rotation space for a given move is called the rotation space curve. We use standard definitions of roll, pitch and yaw or the Euler angles. The roll is defined as the rotation about the zaxis of the base frame, the pitch as rotation about the yaxis and the yaw as the rotation about the xaxis. The rotation matrix transforming the base frame to the tool frame for a RPY point ( <), 6, t ) in rotation space can be derived [Paul 1981] as b, R^ = Rot ((z,.).), (y,9), (x,iÂ»)) cos () COS 9 sin (t) cos 8 sin 9 cos sin 9 cos ^+ sin <^ sin ^ sin () sin 9 cos t cos (j) sin \p cos 9 cos (3.1) Similarly, the transformation of an Euler point (({i, 9, ^p) is given by the following transformation matrix:
PAGE 80
70 Â•^R^ = Rot {z,(t)) Rot (y,9) Rot (z,t) cos <\> cos 9 COS ^ sin (j) sin \) sin (j) cos e cos ^ + cos ^ sin \) sin 9 cos ij) cos <) cos 9 sin ij) sin () cos sin (j) cos 9 sin i) + cos <}> cos sin 9 sin \p cos (j) sin 9 sin (j) sin 9 cos 9 (3.2) The matrix R^ is a transformation relating the base and tool frames. In the following sections, methods for unconstrained and constrained rotational transitions are described. The main emphasis is on separating the determination of the move parameters for translational and rotational parts of a motion. All rotational transitions can be classified into constrained and unconstrained rotations of the manipulator tool. In this section, we classify the constraints on a rotational trajectory into two types of constraints called explicit and implicit constraints. Let us consider the simplest end effector move in Cartesian space, a point to point linear move. A point to point linear move is defined by specifying the position and orientation (referred to as pose) of the manipulator at 2 points in the work space of the robot. The translation interpolation is straight forward since the desired trajectory (linear) is specified. However, the rotational space curve has no constraints in the move specification. Constraints on the rotational space curve can be specified in relation to the translational space curve or Specification of Constraints on a Rotational Trajectory
PAGE 81
71 independently. A description of the constraints to be specified for a welldefined rotational space curve are classified into explicit constraints and implicit constraints. Explicit constraints directly specify the nature of the rotation space curve by giving a number of intermediate target points. Implicit constraints specify the rotational trajectory as a function of the translational trajectory. Explicit Constraints on the Rotational Trajectory Let us assume that on the linear path, the tool orientation is given at one or more intermediate points. The larger the number of points in rotation space for a move, the better the approximation to the rotational space curve (in general). This form of constraint specification is termed explicit, since interpolation points are directly available in the rotation space and the rotational space curve can be approximated by a polynomial interpolation through the intermediate points. Wellknown polynomial interpolation techniques can be' employed for constructing the rotational space curve from a number of via points in rotation space. If only 2 points (start point and destination point) in rotation space for a point to point linear move are given, then one solution would be to approximate the rotational trajectory by a quintic polynomial, since a total of 6 constraints (position, velocity and acceleration) are known at the 2 end points. For the quintic approximation in rotation space, the direction and magnitude of the tool rotational velocity will not be constant, and the intermediate tool orientations with respect to the translational curve will be unpredictable.
PAGE 82
72 If the rotational motion is to be at a constant angular velocity, then appropriate via points (for interpolation) in the rotation space must be specified in order to generate the correct space curve. Implicit Constraints on the Rotational Trajectory Implicit constraints define the rotational motion with respect to the translational tool tip trajectory. For example, one can specify the tool vector always be along the normal to the tool tip trajectory. A typical application where the robot tool tip may be specified along the normal to the translational space curve, is tooltip motion along a circular arc. If the rotation about the tool axis is given as uniform, then the robot tool traces the circular arc with the tool orientation always along the normal and uniform rotation about the tool vector. Implicit constraints must be transformed to generate intermediate points in the rotation space such that a rotation space curve can be generated. Another implicit constraint on tool vector orientation can be specified with respect to the initial and final tool vector orientations. The constraint could be that the intermediate tool orientations must lie in the plane formed by the initial and final tool vectors (both vectors expressed as unit vectors at the origin of the tool frame at the start point). This constraint is used in Paul's scheme for generating the tool rotation [Paul 1979, 1981]. An advantage of imposing the "coplanarity constraint" on intermediate tool vector orientations is illustrated by an example. Assume that a linear move of the robot end effector is programmed such that the initial and final tool vectors are coplanar. If the equivalent axis of rotation approach [Taylor 1979] is used to interpolate rotations, then there is no guarantee that the tool orientation will lie in the plane of the initial
PAGE 83
73 and final tool vectors. Thus, the constraint that the tool always lie in the plane formed by the initial and final tool vectors restricts the freedom of the rotational space curve. There is, however, the choice of the curve within the plane of the 2 tool vectors for the intermediate orientations. If the rotations are to be uniform, then this curve will be linear. Any constraints on the tool vector in Cartesian space have to be transformed into rotation space as interpolation points. Thus, for a point to point move, if the tool vector is constrained to a certain trajectory, each sample point on this trajectory has to be transformed into the rotation space angles. Polynomial interpolation through these sample points for the angles in rotation space yields the rotation space trajectory for constrained tool motion. For an offline creation of a rotational trajectory, interpolation points must be determined in the rotational space based on a technique like the bounded deviation joint paths method [Taylor 1979] that minimizes the rotation error to a certain specified value. In the next section, we describe a conventional translationdominant approach for planning rotational transitions. Since translation and rotation are independent motions, we derive a method for planning independent rotation transitions in the following sections. In order to decouple the rotational motion from the translation, we need an estimate of the rotational tool acceleration in the robot workspace. Equations for the rotational acceleration are developed for a special class of manipulators with spherical wrists.
PAGE 84
74 Construction of Rotational Trajectories Conventional Translation Dominant Approach In this section, we describe some of the disadvantages of a translation dominant approach for computing the rotational move. An example move is chosen to demonstrate the effect of planning a rotation trajectory based on the transl ational trajectory parameters. The example move is a transition of the robot tool from one constant velocity linear segment to another. In this case, the points for starting and ending the transition must be determined. The translation dominant approach employs the constraint on the maximum linear acceleration of the tool to produce a symmetric transition of the tooltip. The same time period is used for the rotational change also. It is shown that the rotational error, defined as the rotational deviation from an intermediate point that the tool misses due to the continuous transition, is directly proportional to the transition time. Thus, if the translation transition time is high, the rotational error will be large, although the rotational change may be small between the points. In Figure 3.1, let PI be the point where the robot starts from rest, moves linearly with a constant velocity v^ to point P2, turns instantaneously at P2, moving linearly with a constant velocity V2 to point P3. From the tool orientations at PI and P2, an axis of rotation, k_ ^ for rotating the initial tool frame at PI uniformly into the frame at P2 is determined [Taylor 1979] (see [Paul 1981] for derivation of k_ ^ ). Another axis of rotation, k. 2 Â» ''^ determined for rotating the frame at P2 into the frame at P3. The move specification requires that the robot tool turn instantaneously at P2 without stopping. Since this cannot be done physically, such motion must be
PAGE 85
75 Figure 3.1 A segment to segment transition
PAGE 86
76 approximated. An accepted method of turning the tool at P2 is to execute a "flyby." A flyby begins turning the tool before P2 such that the tool misses the point P2 but makes a smooth transition to the new segment P2P3 [Paul 1979, Taylor 1979], The degree of the polynomial for the segment to segment transition of the tool tip is reduced by making the transition curve symmetric with constant linear acceleration. The time for the transition is determined from the two linear velocities, v_ ^ and _v 2Â» and the maximum linear tool acceleration, a , as follows: where 2t is the time for the transition. Regardless of the orientation change required between the two segments, the transition time 2t determined from the transl ational motion parameters is also employed for the rotational transition. The main advantage of using the transl ational transition time for the rotational change is the simplicity in executing the motion, since the translational and rotational transitions begin at the same time and end at the same time. The tool orientation is required to change from the axis of rotation k_ ^ and velocity ^ , to the axis of rotation k ^ and velocity u ^ Â• The following equation produces a smooth transition (unconstrained), keeping the orientation and rotational velocity continuous at both ends of the transition. 2t = I V J V 2 /a max (3.3) R(t) = R^. Rot (k_ (t f)^ ojj . Rot (k 0^2) (3.4) 4t ^ 2'
PAGE 87
77 where t' = t T^, being the move time for the segment P1P2 and R2 is the rotation matrix at the "flyby point" P2. The rotation matrix R(t) after t seconds into the flyby segment (t = T^) is given by R(Tj) = . Rot (l<_ a)^T/4) . Rot (k_ 2 5 '^2'^^^^ (3.5) If the robot end effector were to actually pass through the intermediate point P2, the rotation matrix R(t) at t = T^ seconds should be R2, the orientation at the point P2. If ( Â£ p Â£ p a. 1^) denote the 3 column vectors of R(T^) (Eqn. (3.5)), and (n^, 0^, a_) denotes the column vectors of the actual rotation matrix at P2, then the orientation error is defined as (see [Luh 1980b] for derivation) 1=2 iijl ^ a i) (Â£XÂ£^) + (a_xa_^)] (3.6) In order to estimate the rotation error ci as a function of t for Eqn. (3.5), let Ae^ = u^x/A, and AS^ = oj^t/A. For small error analysis, we assume that Ae^ and A92 are small. Expanding Eqn. (3.5), R(T^) = R, ^1 ^^1 k . A9xl 1 k , Ae, yi 1 k 1 Ae^ xl 1 ^2 ^^2 Â•^2 ^^2 ^2 ''2 ^2 ^^2 ^2 ^^2
PAGE 88
78 1 k^i Ae^ Ae^ Ae^ . k^^ Ae^ k^^ A9^ . k^2 ^^2 1 ^2 ^^2 ^1 '\ Â•^1 ^^1 ^2 ^^2 ^1 ' \2 ''2 1 (3.7) All second order terms in Ae are ignored in Eqn. (3.7). Let ^1 = ^1 ' ^2 ^^2 (3.8) '2 ^1 "'l S2 "^2 (3.9) ^3 = ^1 '\ ' \2 ''2 (3.10) Eqn. (3.7) can be written as 1 e = R2 ^1 ' . "2 ' Let \ = [n.. Â£. _a]. Substituting n + e, 0 X 1 X ^2 \ n e, + X 1 Rd^) = "y ' Â°y "z ^ ^1 Â°z ^2 ^ "z ^1 ^ (3.11) D + a e^ n eÂ„o e, + a X x3 x2x3 X V ' 'y '3 Â°y ^3 ' \ ^ ^3 "z ^2Â°z^ 3 ^ ^z (3.12)
PAGE 89
79 The rotational error a is now derived from Eqn. (3.6). (n_ ^ , Â£ ^ , a_ J ) are the 3 columns of Eqn. (3.12). Taking the cross products and simplifying the terms we get a = [{e^. (n_ X o_)^ + e^. (a^ x n_)^ + e^. (o_ x a_)^} j_ + {e^. (n_ X o_)y + e^. (a_ x n_)y + e^. (o x a^)^} j_ + {e^. (n_ X o_)^ + e^. (a_ x n) ^ + e^. (o_ x a_)^} k_] = [e^ a_+ 62 Â£ + 63 n] (3.13) a . a = I a 2 = (e^^ + e^^ + e3^) (3.14) Substituting A9^ = t/4 and ^Q^"" '^2 '^^^ ^(]r]s. (3.8) (3.10), = (t/4) (k^^ 0)^ + k^2 '"z^ ^^^^^ 62 = (t/4) (k^j 0)^ + ky2 "^2^ (^Â•^'^^ 63 = (t/4) (k^^ (0^ + k^2 '"2^ ^^^^^ Substituting Eqns. (3.15) (3.17) in Eqn. (3.14), a^ = (t^/16) (o)^ 2 03^ 0) 2 i 1* Ji 2 "^2 ^ a = (t/4) . (u)^ 2a)j 0)2 I< j . I< 2 + 4 18)
PAGE 90
80 Therefore, the magnitude of the rotational error is directly proportional to the transl ational flyby time x, independent of the arm kinematic structure. A plot of _a versus t in Figure 3.2 illustrates the linear relationship between rotational error and x. The kinematic structure of the robot used for generating the plot of Figure 3.2 is shown in Figure 2.2. The move parameters for the plot are given in Table 3.1. A maximum linear tool acceleration of 1 m/sec^ was employed for the determination of flyby time "2x." Table 3.1 Move parameters for Figure 3.2 JOINT COORDINATES (DEG.) LINEAR VELOCITY ^1 ^2 ^3 ^4 ^5 ^6 Start point Pi 0 0 0 0 0 0 0 m/sec Intermediate 50 0 0 90 45 0 .07 m/sec point P2 End point P5 100 45 45 0 90 45 .15 m/sec Thus, the rotational flyby error for a rotation transition given by Eqn. (3.4) directly depends on the velocity gradient of the linear transl ational path, a dependence not justified by the independence of the transl ational and rotational motions. If x is independently
PAGE 91
81
PAGE 92
82 determined for the rotational transition, the rotational error a can be made smaller and also independent of translational move parameters. In order to determine a rotational flyby time, we need an estimate of the maximum rotational acceleration of the robot tool. Since the rotational acceleration of the tool is obviously a function of the robot pose, a conservative value must be determined such that it is valid for the entire workspace of the robot. One way to determine the instantaneous rotational acceleration of the tool frame is to express the tool frame as a rotation about an axis in space. Let (_k, 6) denote this axis and the rotation about the axis respectively. If the tool frame vectors are denoted as (n, o, a) , then 9 is given as e = tan"^ /(o^ aj^+ (a n ) ^+ (n o ) ^ ^ ^ (3.19) n + 0 + a 1 ^ ' X y z T T T where n_ = (n^, n^, n^) , Â£ = (o^, o^, o^) and a_ = (a^, a^, a^) . Since (n[, Â£, a_) are functions of the joint angles (9^ .. 9g), Eqn. (3.19) can be written as 9 = f (9^ .. 9g) . (3.20) Differentiating Eqn. (3.20) twice with respect to time, and substituting for the maximum values of the joint rates and accelerations, we can obtain a value for the instantaneous tool acceleration when all the joints are driven at their maximum rates. Since the rotational acceleration is a function of the arm pose, the magnitude of the rotational acceleration varies at different points in the robot workspace. The equation for the rotational acceleration
PAGE 93
83 obtained by differentiating Eqn. (3.20) is a nonlinear function of the joint angles, velocities and accelerations with a large number of coupling terms in the joint parameters. Therefore, a mathematical estimation of a maximum magnitude of the rotational acceleration that is usable in the entire robot workspace independent of the arm pose and velocity, is nontrivial by this approach. In the following sections of the chapter, we analytically derive the limits on the tool rotational acceleration by decoupling the rotation into two rotations about the robot wrist. Construction of Independent Rotational Motions In this section we describe methods for producing independent tool rotations. In order to determine an independent move time for the rotations, we need an estimate of the maximum rotational acceleration as mentioned earlier. For certain unconstrained tool rotations, the limits on the maximum joint rates and accelerations can be directly employed to determine a rotational move time. For many other rotational moves, we need an estimate of the tool rotational acceleration, which is a composition of all the joint rates and accelerations. This estimate of the tool rotational acceleration must be valid for the entire robot workspace, independent of the arm configuration. We derive the equations for the estimation of the rotational acceleration of the tool. The derivation is specifically for the most common industrial articulated robot geometry manipulators with spherical wrists. The spherical wrist decouples at the "wrist" joint of the articulated manipulator into the so called position and orientation degrees of freedom. This property is used to decouple the rotational motion into
PAGE 94
84 rotation due to the orientation angles, and rotation due the positioning angles. The decoupling is employed for planning independent rotational motions for the orientation angles, 6^, 6^ and Sg . An estimate of the rotational acceleration in the "rotation space" formed by the orientation angles is determined. Due to the simplification afforded by the decoupling, the exact nature of the rotational acceleration in the robot workspace is easily visualized and analytically proven. We completely analyze one type of a wrist, namely the "Euler" wrist, for maximum rotational velocity and acceleration of the tool. The results of the analysis are applied to a simulation example that computes independent rotational transitions. The plots of the joint angles versus time show the improvement in the joint trajectories due to an independent rotation trajectory computation as compared to a translation dominant approach for trajectory determination. The next section describes the estimation of a rotational acceleration of the tool in the robot "rotation space." Planning Constrained Rotation Transitions Estimation of Acceleration For Spherical Wrist Manipulators For a manipulator with rotary joints, the rotational velocity and rotational acceleration of the tool is limited by the joint velocities and accelerations. Table 2.3 shows the fullload joint velocities and accelerations of the manipulator of Figure 2.2. The waist rotation 6^ has the smallest full load acceleration since for most moves, the loading on joint 1 is the maximum. We observe from Table 2.3 that the fullload velocities and accelerations of the last three rotations, 6^, 9g and 9g, are approximately twice as high as the velocities and accelerations of the first three rotations, 9,, 9Â„ and 9. This is
PAGE 95
85 typical of articulated robots with a spherical wrist. In this section, we decouple the manipulator degrees of freedom at the wrist into positional and rotational degrees of freedom. The decoupling of the translation and rotation at the wrist yields a less conservative estimate of the rotational acceleration as compared to the case where the decoupling is not possible, since the rotational velocity and acceleration constraints are determined independently for the first 3 and the last 3 degrees of freedom. Decoupling rotational motion at the "wrist" joint For most 6 degrees of freedom robot manipulators, the wrist joint (Figure 3.3) can be regarded as the decoupling joint between 3 positioning degrees of freedom and the 3 orientation degrees of freedom. The first 3 joints are called the positioning joints and the last 3 joints the orientation joints. The decoupling at the wrist has been employed to simplify the computations in the reverse (or world to joint) transformation, and the inverse dynamics for the robot [Hollerbach and Sahar 1983]. We define the 3 orientation degrees of freedom as the basis for a tool "rotation space" for a robot with a spherical wrist. The rotation space is created by the joint angles 6^, and Sg of Figure 3.3. A coordinate frame defined at the wrist joint is the reference frame for the rotation space. Note that the wrist frame changes between successive robot poses. But since the wrist frame is not a function of the orientation angles, and can be determined independently from the tool pose, the rotation space angles 9^, 9^ and 9g are not changed. We determine the velocity and acceleration constraints in rotation space and separate rotational motion planning into tool rotation in rotation space and the wrist rotation about the base frame. Since the first 3
PAGE 96
86 joints are basically the positioning joints, we can anticipate most of the tool vector rotational motion planning in the rotation space. In order to demonstrate the decoupling at a spherical wrist, we consider the manipulator of Figure 3.3 as an example. The "A" matrices [Denavit and Hartenburg 1955, Paul 1981] for this manipulator can be derived as A, = Ao = A. = A, = A. = 0 h ^1 h 0 h ^2 (3.21) 0 1 0 h 0 0 0 1 ^2 0 Cg I3 ^2 ^2 0 Sg I3 (3.22) 0 0 1 0 J) 0 0 1 ^^3 0 C3 1 4' '2 0 ^3 4 (3.23) 0 1 0 0 _p 0 0 1 0 ^4 0 ~ ^4 0 ^4 0 (3.24) 0 1 0 h 0 0 1 Â•^5 ^5 0 0 ^5 0 0 (3.25) 0 0 1 0 0 0 0 1
PAGE 97
87 '^6 ^6 0 0 H ^6 0 0 (3.26) 0 0 1 0 0 0 1 We will show that the velocity of the tool referred to the base coordinate frame can be decoupled into (1) a rotational velocity of the tool vector with respect to the forearm (between elbow and wrist) and (2) a velocity (rotational and linear) of the wrist referred to the base frame. [Featherstone 1983, Hollerbach 1983]. It is usual for a robot motion specification to contain the linear velocity of the tool and the rotational velocity of the tool frame expressed in the fixed base B B frame. Let ^ and o) ^ denote the linear and rotational velocities of the tool vector. From Fig. 3.3, (3.27) Â£w = ^t ^ Differentiating Eqn. (3.27) B Â• B Â• B Â• Â£w= ^t^t (3.28) where [T6] = ^^^.^2...I^Q and Â£=[00 l]""".
PAGE 98
88 Figure 3.3 Decoupling end effector velocity at the wrist joint
PAGE 99
89 Substituting Eqn. (3.29) in Eqn. (3.28) B Â• B Since 2. I ^ Â— t ' 'iÂ„ = "vj^^x8[Tj]z (3.31) Eqn. (3.31) gives the velocity of the wrist. We now determine the velocity of the tool with respect to the "wrist frame". The wrist frame B[W] is defined by the product of the A matrices, A3 and A^. The rotational velocity of the tool frame is the vector sum of the wrist rotational velocity and the tool rotational velocity with respect to the wrist, all velocities referenced to the base frame. B B ^ B W /o oo\ 0)^= i;)3+w^ (3.32) B W where ^ is the rotational velocity of the tool from the wrist and B ... 0) 2 IS the wrist rotational velocity (due to e^, 9^ and e^). Multiplying Eqn. (3.32) by ^R^^, we transform all velocities in Eqn. (3.32) to the wrist frame. W W ^ W W (3.33) ^t = ^3^ WWW w ^t= ^t ^3 (3.34)
PAGE 100
90 (3.35) W 0) _ is determined as follows: Bp _ B W (3.36) W Substituting for P ,, from (3.31) and. is knov/n from the joint coordinates, we can solve for the 3 elements of iil 3 Eqn. (3.36). Then ^lu _ is given as Therefore, we can now determine the tool rotational velocity with respect to the wrist frame. Thus, the tool frame rotational velocity, o)^, can be divided into 2 components. One is the tool rotational velocity referred to the B W wrist frame, w ^ , also called the velocity in rotation space and the P other is the wrist rotational velocity, _co ^ , referred to the base frame. Since the 2 rotational velocity components are independent, constraints on the rotational velocity of the tool frame can be transformed into constraints on the wrist rotational velocity and the tool rotational velocity from the wrist. Rotational Acceleration of the Tool Vector in Rotation Space The magnitude of the tool rotational acceleration in rotation space is limited by the fullload velocities and accelerations of the 3 degrees of freedom in rotation space. In this section, we derive the W (3.37)
PAGE 101
91 maximum "usable" tool rotational acceleration in rotation space. The maximum usable magnitude of the tool rotational acceleration is independent of the arm configuration. Let W denote a coordinate frame at the wrist joint. For the manipulator of Figure 3.3, we define W as follows: [W] = [Al] [A2] [A3] [T (z.lg)] (3.38) where [Al] ... [A3] are the A matrices given by Eqns. (3.21) (3.23) and T (z, Ig) is the translation from the "elbow" to the wrist joint. w w * The rotational velocity w and the rotational acceleration u are given in terms of the 3 rotation space rotations as ''^OJ = 9 z + [Rot (z, 6 ) ^] e + ~ ~ . (3.39) [Rot (z_, 64) Rot (y. 6g) z] 8g i!i = 64 1+ ^4 ^5 [z. X Rot {z, 9^) + Rot {z, 9^) X Sg + 64 8g [z_ X Rot (z_, 9^) Rot ( 1' 9^) z ] + 9g 9g [Rot {z, 9^) {y x Rot (x, ^s)) 1 ] + [Rot {z, 9^) Rot (x, 9g) z ] 9g (3.40) where 7 = [0 0 l]"*^ and ^ = LO 1 0^^ . The large number of "coupling" terms in the joint angles and accelerations, and the dependence on the tool pose make Eqn. (3.40) 6
PAGE 102
92 difficult for a direct evaluation of the limiting magnitude of '^uj' . In order to simplify the evaluation of a maximum tool rotational acceleration that is independent of the tool pose and is, therefore, valid for all tool configurations in rotation space, we divide the rotation in rotation space into 2 components. One component is a pure rotation about the tool axis, the constraints on which are known from the fullload constraints on the tool rotation about the tool axis (last link). The second component is due to the remaining two rotations, 8^ and 85. Thus, our problem is reduced to the determination of constraints on the rotation due to 8^ and 6^ followed by a combination of these constraints with the constraints on the tool axis rotation, to yield the constraints on the overall rotation. The two accelerations in rotation space can be thought of as the tool approach acceleration and the tool orientation acceleration, since 8^, change the tool approach and 8g varies the face plate orientation. Thus, if the rotation of the tool in base frame for a move is divided into approach rotation and rotation about the tool axis [Paul 1979, 1981], transformation of these 2 rotations into the rotation space directly yields the divided rotations. Thus at any particular instant of time. Rot (V \) = Rot (\ , 3) Rot (\ , Sg) (3.41) a b where \ is the known axis of tool rotation in the wrist frame, 8^ is the known rotation angle about this axis, '^k^ is the decoupled axis of W ^ rotation due to 8^ and 85 and k_ is the tool axis itself. The solution for \ , 3 from Eqn. (3.41) is considerably simplified if we a y transform the equation to the tool frame "T" such that k_ becomes the b
PAGE 103
93 zaxis and \ is an axis obtained by rotating the tool frame yaxis ^ a radians about the tool frame zaxis [Paul 1981]. Transforming Eqn. (3.41) to the tool frame, Rot Ck , e^) = Rot (^k_ , e) Rot Cz , 9^) (3.42) "Â•"k = (St c^\> 0)"^ , (3.43) "^z = ( 0 0 1)"^ (3.44) where s^, c<) denote the sine and cosine of ^ respectively. Expanding both sides of Eqn. (3.42) and solving for ^ and S , we get \li = ATAN2 [(k k vers w k sin u) , (k k vers oj + k sin u)] r LVy2 X "^xz y (3.45) 2 sin g = S'l' ci) Vers 9g(k^ vers a)+ cos u)) + (cii^^ vers 0g + cGg) (k^ k^ vers co + k^ sin w) (3.45) sij) S9. (k k vers lo k sin u) ^ 6 ^ X z y ' cos 3 = St cip vers Sg (k^ k^ vers oj k^ sin u) 2 2 + (ct vers + cSg) (k^ vers u + cos oj) (3.47) St se^(k k vers tu + k sin to) 6' y z X e = ATAN2 (sin 3, cos g) (3.48)
PAGE 104
94 Expressed in the wrist frame, the axes of rotation are \= (R4 Ag)"^ [C^ of (3.49) d = Rot (\ , g) (3.50) b a where '^z = [0 0 1]"^ (3.51) and R4 is a pure rotation about the wrist frame zaxis by angle 6^, In the wrist frame. '^k (0 = '^k e + Rot ("^k , e ) z Qr (3.52) a a ^ Differentiating Eqn. (3.52) \m = \ B" + l^k i X Rot (^'ji , e ) z e ] a a a + [Rot (\ , B) z_ ] Gg (3.53) a Let us analyze Eqn. (3.53) for the magnitude of 00 . Notice the fewer terms in Eqn. (3.53) as compared to Eqn. (3.40). Let '''k, = [a^ a^ 83]^ (3.54) and Rot (\ ,3) z_ = [b^ b^ b^']^ (3.55)
PAGE 105
95 The vectors in Eqns. (3.54) and (3.55) are unit vectors. Substituting Eqns. (3.54) and (3.55) in (3.53), taking magnitudes and simplifying, we get The only negative term in Eqn. (3.56) can be the last term whose coefficient is the dot product of the unit vectors given by Eqns. (3.54) and (3.55). The minimum value of the coefficient of this term is 1. The following observations can be made from Eqn. (3.56): (1) the bound on the maximum fullload, configuration independent value of a) is obtained when 9, = 9" = 0 or 3 = 3" = 0, (2) since the maximum full0 0 load bound on 9" is known, we now have to determine the maximum 6 configuration independent value of 3". The smaller of 9g and 3" yields the required magnitude of o), (3) the actual magnitude of 0) as shown by Eqn. (3.56) is always greater than or equal to the magnitude of the fullload configuration free values of either 6^ or 3". We now determine the maximum fullload configuration independent magnitude of 3". Let tz denote the tool vector in the wrist frame, tz is given by the two rotations 9 , 9 in the wrist frame as where [t^ t^ t^] denotes the initial (or a reference) tool orientation in the wrist frame. Without loss of generality, we can assume [t , t^. (3.56) W. 'tz = Rot ("z, 9^) Rot (V 9^) [t^ ty t^]^ (3.57)
PAGE 106
96 t,] = [0, 0, 1], or the initial position of the tool in the wrist frame is along the zaxis. Then, tz ^4 ^5 ^z ^4 H S ^z (3.58) where s^, c^, s^, represent the sines and cosines of 9^ and 9^ Differentiating Eqn. (3.58), 'tz' = ^4 ^5 ^z ^4 H ^ C4 C5 1^ ^4 ^4 H ^z Sr t 5 z _^5^ (3.59) = J [e^ e^] (3.60) where J is the 3x2 Jacobean matrix. Now, Wtz= B X '^tz . (^^1) Differentiating Eqn. (3.61), 3" X ^^tz = ^tz" e X e X "^tz (3.62) Taking magnitudes on both sides of Eqn. (3.62), I 1" X ^tz I > I ^tz" I I i X _e X V I (3.63) I
PAGE 107
97 e" I I '^tz 1 sin T > 1 '^tz" I I i X i X ^tz I (3.64) I'tz" I _e X _Â§ X tz_ l^^tz I sin Y I'^tz I sin y where y is the angle between and t_z. Differentiating Eqn. (3.60), Y * 0 (3.65) (3.66) Evaluating Eqn. (3.66), H Â°1 'l^ U Â°2 (^4 Â«2 '5 ^4 "1 '4) '4 Â• + (C4 % S4 e^) 85 C4 Â«i ej + S^ + {C4 65s^ O^} 9^ Â• Â• Â• + {S^ ct^ 65 + 64} Sj. (3.67) where a, = Sc t 1 5 z (3.68) Â°2 = S ^z (3.69)
PAGE 108
98 Taking the magnitude on both sides of Eqn. (3.67), * Â• + (a^ ej+ 2 9^ (ct^'s^^ e^^)^ (3.70) W 2 In order to find the minimum  tz"  when the joint rotations 6^ and 6g are driven at their maximum full load acceleration, we consider the following cases. (1) V = 0 iVl^ = (e^"^ + 6"^) (3.71) 2 2 2 2 where = + = (tool length) (2) = 0^ = 0 iVl^ = sin^ (9 1+ ef ) (3.72) where y is the angle between the zaxis of the wrist (also the axis of rotation) and the tool vector. Note that the y in Eqn. (3.72) is the same y of Eqn. (3.65) . It can be easily shown that any other combination of 9^, 6^, 9g, 9^ is either greater, or can be derived from Eqns. (3.71) and (3.72). Thus, the bound on the maximum configuration independent value of I ^tz"  occurs when either 9^ does not move or when 9^ is fixed.
PAGE 109
99 * * \ I The second term,  1 x _3 x tz_  , in Eqn. (3.65) is also bounded on its maximum tool configuration independent value by either = 0 or = 0 as shown below. 0 _e X i X *^tz I = _3^ tz sin k (3.73) where < is the angle between _B and tz_. We determine by taking magnitudes on both sides of Eqn. (3.61), Ttz' I = ill \tz I sin < (3.74) 3=Â— (3.75) we derive I'^tz^'  from Eqn. (3.59) as I \z' 1^ = e/+ (a2+ al ) (3.76) where a^, are given by Eqns. (3.68) and (3.69). Eqn. (3.76) shows that ^tz_'^ is a sum of two squares. If both terms on the right hand side of Eqn. (3.76) are zero, then we get the trivial solution, I'^tz ' I = 0. The lower bound on the maximum value of ^tz_'  is given by making one of the two terms zero at a time. Thus, (3.77)
PAGE 110
100 l^tz' 1^ = T,_^ sin^ K 6^^ 0g= 0 (3.78) We conclude from Eqns. (3.73), (3.75), (3.77) and (3.78) that the * * w maximum configuration free (lower bound) value of ll x _3 x tzj is Â• Â• given when either e^= 0 or 9^= 0. lixix^tzl = (T^ e^^) /(sin k) e^= 0 (3.79) or li x _3 x '^tzl = 9^ sin^< 9^= 0 (3.80) Substituting Eqns. (3.71), (3.72), (3.79), (3.80) in Eqn. (3.65), e" > [ (9g^ + 6'^^) / sin Y e^^/ sin^ ] = ej = 0 (3.81) > [ (64^ + Qf) e^^] % = ^5=0 (3.82) Note that sin < = sin y due to the conditions imposed on the axis of rotation. In Eqn. (3.81), sin y = 1 because the angle between the axis of rotation and the tool vector is always ti/2 radians due to the 9^ = 0 condition on rotation. Therefore, we can rewrite Eqn. (3.81) as 1 3" I > 1{Q^^ + ef ) Q^:\ 0^ = ej = 0 (3.83) The smaller of the two terms on the right hand sides of Eqns. (3.82) and (3.83) gives the limiting magnitude of the maximum _3". Thus, returning to Eqn. (3.56), the configuration independent magnitude of the angular acceleration of the tool,  a) , is limited by
PAGE 111
101 Â• the smaller of 9" or g". Since g" is again limited by the slower of 0 6" and e" we conclude that the tool vector rotational acceleration in 4 b rotation space is limited by the slowest rotational joint. Thus, summarizing the results of this section, we decoupled the rotation in the rotation space into 2 independent rotations in order to simplify the estimation of the rotational tool acceleration. We derived an expression for the rotational acceleration o) in terms of e", e, and 6" where 3" is a function of the first two Euler rotations, 0 b (j) and e, and 8g is the acceleration of the rotation about the tool axis itself. The equations for u, 3" are simple enough to be computed in realtime for an instantaneous acceleration. In other words, given a tool pose, rotational velocity oj, and joint velocities and accelerations, we can determine the instantaneous value of the tool rotational acceleration u. Since the value of to continually changes with the tool pose, we determined a magnitude of u that is less than or equal to the maximum value of u) for any tool pose in the wrist frame. We proved analytically that a) is given by the rotational acceleration of the slowest joint in the rotation space. Point to Point Unconstrained Transitions For a point to point unconstrained transition, the position and velocity at the two ends of the transition interval are known. We have to determine an independent move time for the tool orientation to change from its initial frame and velocity to the destination frame and velocity. Let us assume that the tool is rotating about an axis k_ ^ with an "angular velocity _k ^ . Let it be required that the tool change its axis of rotation to k_ ^ and angular velocity to
PAGE 112
102 "2 Â— 2* '^^ '^l start point of the transition and P2 the destination point. At P^ and the linear and rotational velocity constraints, Â± ^t ^ 2 "^1 Â— 1' '^2 Â— 2 ''^^ easily transformed to the velocities of the orientation angles of the spherical wrist manipulator by initially decoupling the velocities at the wrist, as in the preceding section, and then solving for the joint velocities in rotation space [Featherstone 1983, Hollerbach and Sahar 1983], Let Â• Â• Â• Â• Â• Â• (^, e^, 4)^, e^, ^^)^) and {^^, e^, *2' denote the boundary conditions at P^ and P2 respectively for the rotational transition in rotation space. From the specifications of the joint motors of the orientation angles, we know (9^,Â„ , 6" , , ^"^ ), the max max max servo accelerations at full load. If a cubic polynomial is used for each of the orientation angles (in order to keep the position and velocity continuous at P^ and P2), we can determine a move time "Tp" for each orientation angle such that the constraint on maximum acceleration is not violated. Let 3 2 f(u) = c^ u + u + c^ u + Cj (3.84) denote a cubic polynomial for one of the orientation angles, "u" is the normalized time, given as u = (t t._i)/T._^ (3.85) Ti.l=^i ^il (3.86)
PAGE 113
103 where T^_^ is the time for transition between PP2 i^i'^z denoted as the ith time interval in the series of time intervals for the robot move). Let (y^ ^, Y^, Y^) denote the boundary conditions at P^ and P2. The coefficients of f(u) can be derived in terms of the boundary conditions as C3 = 3 (Ay.) (y^ + 2 y._^) T._j (3.88) C4 = 2 (Ay.) + (y^. + T._^ (3.89) where Ay. = y. Y^_j. Since the maximum acceleration for f(u) can occur at either u=0 or u=l, we evaluate T^,^ at both ends of the ith time interval (P^ P2). The equation for the acceleration y" is obtained by differentiating f(u) twice. y" T?_^= 6 c^ u + 2c2 (3.90) Substituting for c^ and C2 and solving for T^.^ at u=0 and u=l, we get (4y. 1 + 2y. ) t / 4t^. ^ + 2y.)^ + 24 y" Ay. (3.91)
PAGE 114
104 u=l (2 Y^_^ + 4 Y.) t / (2 + 4 Y^)^ + 24 y" Ay2y" (3.92) Only one of the above values for T^_2 gives a positive real root. Thus, the move time for a point to point rotational transition can be determined independently by computing T^_^ for each of the 3 orientation angles, and using the minimum value. Independent Rotations An Illustrative Example In order to demonstrate the application of the results of the previous sections, we consider a robot move where the conventional translation dominant trajectory planning techniques fail to produce a smooth rotation of the robot tool. The robot used for simulation is shown in Figure 3.3 The move for simulation, shown in Fig. 3.4, is specified as follows. The robot end effector should move continuously along a linear path from a point P^ and initial orientation R^ to an intermediate point P2 with rotation R2 and, finally, to a point P3 with orientation R3. The linear velocity of the tool tip (assumed to be the tip of the last link) along P1P2 and P2P3. is a constant 0.15 m/sec. The points P^, P2 and P3 are chosen such that they are colinear. Since the linear velocity along P1P2 and P2P3 is the same in magnitude and direction, no smoothing of the translational trajectory is required at P2. The rotations at P^, P2 and P3 are chosen such that the rotational velocity of the tool is quadrupled along P2P3 (over the one along P^P3). Figure 3.4 shows the move and move parameters.
PAGE 115
105 Figure 3.4 A simulation move
PAGE 116
106 A wellknown technique for position and rotation interpolation along a linear end effector trajectory [Paul 1979, 1981] is employed for generating joint coordinates at a fixed sample rate on the linear tool tip trajectory. A sample rate of 50 ms is chosen, since this value is representative of the typical sampling rate on industrial robot controllers. The rotational motion is divided into approach vector rotation and approach vector orientation. The first rotation serves to rotate the tool vector from its initial orientation to the final orientation. The second rotation, which is about the tool axis, aligns the "faceplate" axes (the 2 axes of the tool frame perpendicular to the tool vector) from their initial orientation to the final orientation [Paul 1979, Paul 1981]. The continuous linear motion from P^ to P3 is computationally divided into two linear moves, ^1^2 ^2'^3' '^^^ "flyby" time at P2 for the continuous move using a translation dominant approach is given as: ll 1 "111 T = i ^ = 0 (3.93) max where _v_ ^ is the velocity along P1P2 and P2P3 and a^^g^ is the maximum linear acceleration. Thus, at P2 no smoothing is done. Since the rotational velocity is quadrupled on the segment ^2~^3' ^ J'^'^P ^'"^ orientation angles will be observed at P2. The translation dominant approach cannot smooth this jump in the orientation angles. The plots of the joint trajectories in Figures 3.5 to 3.10, where the solid line trajectory indicates a translation dominant move planning, shows a jump in the angles 6^, and 8g. The plots for 9^, and 6^ show that the jump due to an instantaneous change in the rotational velocity does
PAGE 117
107
PAGE 118
108 i/i 0) i. o ta so 1Oi to S> o +J CVJ o 0) l> lÂ»Â•rCU o Â•oo Â£1 O (U >>t= o l> 4U O so CO Sn CD
PAGE 119
109 to o +> CO u (U Â•> 4C MÂ•rQ) O o (U >)C S+J o o o CO > io CO
PAGE 120
110 u 01 C O c ra M c Â•a Cl a cn a
PAGE 121
Ill >, >> So Q (J OJ > o o B E 1/1 +J c c o n3 Â•rC +J Â•r(O B +J o o o s+> o c (U XI (0 c cu 1/5 Q. c O) CO o ic +> Â•rc
PAGE 122
112 so 4> o dJ Â•<Â—> i> o >) so 4J o OJ Â•<> ro slJ > O Â•r(/I (U +) S. (O o o 4t/1 S> o 4J vo (J (U t> 4C Â•rOJ O Â•r)T3 c o >>J= $+J o +J MU O QJ Â•13 iO CO sCD
PAGE 123
113 not produce significant jumps in the first 3 angles, since the end effector position and linear velocity are continuous. II II The rotational accelerations, B (Eqns. (3.82), (3.83)) and Og , are employed to smooth the discontinuity in the rotational velocity at At ?2, let Aiiiapj ^Â— or differences in the tool approach and orientation velocities along P1P2 ^nd P2"^3' rotational velocity jumps, Au and Au are transformed in to velocity jumps in "~ a p Â— or the wrist frame, ^Au) and ^Aui . Two transition times t,,, and t^, Â— ap Â— or ap or' for smoothing the velocity jumps at the maximum permissible acceleration (independent of the tool pose) are determined as follows: The greater of the two transition times, x and x , is the time ap or' for smoothing the jump in the rotational velocity at P2. Let x^ denote the greater of x^^ and x^^. Then, the rotational velocity is changed smoothly from x^/2 seconds before P2, to x^/2 seconds after P2. The intermediate rotations are given by the following equation. R(t) = R2 [Rot [k ^p^, {(x^ t)2 co^p^} / 2x^] Rot [k ^^''orl^ / ^^'^ (iap2' ^Sp2/ Rot (k ^^2' V2/ 2^)1 0 < t < x (3.96) r
PAGE 124
114 The dashed lines in Figures 3.5 to 3.10 show the joint trajectories due to smoothing in rotational space by Eqn. (3.96). A Note on the Wrist Rotational Velocity in Base Frame In the section on planning constrained rotation transitions, we showed that the tool rotational velocity can be decoupled into rotational velocity of the tool about the wrist frame and the rotational velocity of the wrist about the base frame. We also described the determination of constraints on the rotational velocity of the tool in the wrist frame. In this section, we briefly address the determination of the constraints on wrist rotation about the base frame. Since the first 3 joints are primarily the "positioning" joints, and the limitations on the tool linear velocity and acceleration are known, we know that the linear velocity and acceleration limits of the tool tip apply to the wrist also. The linear velocity and acceleration constraints, that are valid for all moves of the tool, become those of the wrist when the orientation angles do not change, and the wrist trajectory is parallel to the tool tip trajectory. The wrist rotational velocity can, therefore, be estimated from the equation: 'iw = (3.97) e where r is the wrist position vector in the base frame. The worst w case value of I i!i ,^1 can be determined by finding the closest position of the wrist to the robot base frame. The rotational acceleration can be estimated by differentiating Eqn. (3.97).
PAGE 125
115 For manipulators with the kinematic structure of Figure 3.3, the wrist rotational velocity is primarily limited by the "waist" rotation 6^. This joint is generally the slowest of the 6 joints in Figure 3.3, because for most moves, the loading on joint 1 is the maximum. Thus, another estimate of the limit on the wrist rotational velocity is the maximum full load speed of the waist joint. The wrist rotational acceleration is the rotational acceleration of the waist joint. The wrist rotational velocity and acceleration thus estimated must be greater than, or equal to the rotational velocity and acceleration R R determined from Eqn. (3.97), since  _v ^ and  a_ ^ are the estimates of the linear velocity and acceleration that are independent of the arm configuration and the move. For a robot move that has a translation and rotation, if the translation trajectory is continuous in position and velocity, a discontinuity in the rotational velocity will mostly appear in the orientation joints (9^, 9^ and 9g in Figure 3.3). This is illustrated in the simulation example of the preceding section. However, if a move is purely rotational, as in rotation of the tool about one of the tool frame axes keeping the tool point fixed (a common feature on most industrial robot "teach pendants"), the wrist rotational acceleration may be the determining factor for the move time. In general, for any robot move, the limitations on the rotational acceleration must be checked in the base frame for the wrist, and in the wrist frame for the t joI . The greater of the two move times limit the rate of change of the tool rotation.
PAGE 126
CHAPTER IV TRAJECTORY SPECIFICATION AND LOAD DISTRIBUTION FOR CLOSEDLOOP MULTIMANIPULATOR SYSTEMS In Chapters II and III, we described a new technique for online polynomial interpolation in joint space and developed the concept of independent robot motion planning in rotation space. Thus, given a multimanipulator task, we can plan constrained motions (translations and rotations) of the end effector such that the motion does not demand excessive joint rates or joint accelerations for either translation or rotation dominant moves. With the "feasible" motion planned by independent translation and rotation trajectories, we can generate joint trajectories between successive sample periods on the end effector trajectory employing the online cubic splines technique of Chapter II. If the robots are working on independent subtasks, a fast and unconstrained tool transition between work points can be executed efficiently with a controlled startup and setdown jerk by the joint model of Chapter II, which clips both velocities and accelerations of the nonpacing joints. In this Chapter, we address one of the problems in the coordination of a "closedloop" multirobot task, namely, the load distribution at the end effectors in the closedloop. A large number of tasks, from small item assembly to displacement of large objects, can be made faster and more efficient with coordinated movements of multiple robots working in a closedloop. In a closedloop task, as in the displacement of a long rod by two manipulators, an active distribution of the forces and moments at the end effectors is 116
PAGE 127
117 required by most control techniques for the determination of the motor torques of each manipulator in the closed loop. Several control schemes have appeared in the literature for closedloop control of multiple manipulators in a closed chain, or mechanisms similar in configuration to the closedloop multimanipulator system [Hemami and Wyman 1979, Ishida 1977, McGhee and Orin 1976, Orin and Oh 1981]. For discussion, we broadly classify the control techniques into 3 categories: (1) force control, (2) hybrid control and (3) computed torqueposition control. In a force control scheme, the joints are servoed on force feedback instead of the usual position and velocity in a traditional PID loop. The system predicts the joint torques for a given trajectory from force sensors located on the joints or at certain parts of the manipulator. The joint motors are servoed on force feedback and the system attempts to build up the torques on the motors to counter the sensed forces. Hybrid control of a manipulator is defined as a control scheme that employs position servoing on some joints and force servoing on others [Paul and Shimano 1976, Raibert and Craig 1981]. The primary application of hybrid control as in [Paul and Shimano 1976, Raibert and Craig 1981] is for controlling compliant motions of a robot end effector. The combined forceposition servoing technique of hybrid control has been applied to multimanipulator control also. The idea is to create a master/slave control [Ishida 1977] where the master is position servoed, and the slave .s force servoed. The master slave idea works well as long as the attachment between the object and manipulators is absolutely rigid, and no compliance is possible. If there are degrees of freedom between the object and an end effector, then the
PAGE 128
118 sensed forces on the effector will not accurately predict the notion of the system. In this case, the slave is bound to loose position accuracy. The computed torque control method determines the torque requirement of each motor by a dynamic model [Luh 1980]. The torques on each joint are constantly updated from the position and velocity information from the trajectory, and feedback from the servo. Since the joint torques are a function of the arm pose, the computed torque control technique yields good position control. Thus, in case of slippage at the end effectors, the current robot position is updated from position feedback and incorporated into the new computation of the joint torques. Since the joint torques are a function of the external forces and moments on the effectors, we require a model to predict the forces and moments on each end effector in the closed chain. At each sample point on the object trajectory, the object load must be distributed among the effectors holding the object. Research on closed kinematic chains in locomotion systems like bipeds and hexapods has shown that the problem of load distribution for a given trajectory of the closed chain is under specified [Orin and Oh 1981, Seireg and Arvikar 1975, Williams and Seireg 1979]. Linear programming using the Simplex method, with minimization of power was implemented for solving the load distribution problem for a hexapod locomotion vehicle [Orin and Oh 1981]. The computation of torques for driving the hexapod on a given trajectory was 1.37 seconds per sample point on a PDP11/45. In this Chapter, we describe the task and constraint specification for a closedloop multimanipulator task, and develop two linear models for load distribution in the closedloop. Both models employ the space
PAGE 129
119 curve trajectory of the object to create coordinate frames with axes along the space curve tangent, normal and binormal directions. It is shown that the use of the space curve coordinate frames simplifies the constraints on the load distribution problem. The first of the 2 linear models simplifies load distribution by separating the forces for translation and moments for rotation of the object. This model is applicable to tasks performed at a speed low enough for the angular velocity effects on the transl ational force to be ignored. The simplicity of the algorithm allows forces and moments to be balanced at a fast rate during the actual motion of the system. The second model takes into account the effects of all determinate forces on the object and generally would require a large table when linear programming is employed for solution. Both methods do not directly account for the interactive forces due to the simultaneous motion of all the manipulators. Since forces will be redistributed as the system moves, it is assumed that the error in force balance at a certain time instant can be sensed, and incorporated into the next force balance, thereby avoiding the possibility of a steady error build up. Planning a MultiManipulator Task A multimanipulator task, like any other robot task can either be preplanned or executed online. If the task is preplanned, then the object and end effectors trajectories are computed and stored. The task planning procedure, in addition to planning an "optimal" trajectory [Luh 1981], must ensure that (1) the trajectories of the manipulators do not cause a collision among the links of the manipulators or other static
PAGE 130
120 obstacles, (2) each end effector trajectory is within the usable workspace of the respective manipulator, and (3) the trajectory precludes singularity points and limit points in the manipulator workspace. Planning collision free manipulator trajectories is a well researched subject and several techniques for collision avoidance have appeared in the literature. One of the first published works on obstacle avoidance is by Udupa [1977]. Udupa first formulated the obstacle avoidance problem in terms of an obstacle transformation that allows treating the moving object as a point. Research on obstacle avoidance at the MIT Artificial Intelligence Laboratory has provided greater insights into planning obstacle free trajectories [LozanoPerez and Wesley 1979, LozanoPerez 1981]. Techniques for planning optimal trajectories by workspace constraints of a manipulator are also known [Luh and Lin 1981]. Planning a trajectory that precludes singularity points [Taylor 1979] in an articulated manipulator's workspace is not an easy task. A singularity point causes excessive joint motions in some joints of the manipulator for relatively small displacements of the end effector in Cartesian space. Thus, when an end effector passes through a singularity point, the sudden increase in joint rates may result in an unpredictable and jerky motion of the end effector. In a closed loop multimanipulator task, this could mean excessive forces on the object and might even cause the object to slip and drop. There is no known straightforward method for checking and handling singularity points in the manipulator workspace. If the planning and execution of a multimanipulator task are simultaneous, then considerations such as constructing minimalwork trajectories between 2 points are important. For example, there are
PAGE 131
121 usually several ways of executing a motion involving a translation and rotation. The rotation can be uniform or nonuniform. The determination of an optimum motion strategy should take into consideration the limitations on individual manipulator workspaces and physical limits on the manipulator joints such as the maximum motor torques. The next two sections list the task and constraint description parameters for planning a multimanipulator task and describe the trajectory specification for a closedloop task. Task and Constraint Description The following is a list of parameters that must be specified as task description for object manipulation by multiple manipulators. The task and constraint description parameters are used both for trajectory planning and forcetorque balance. (1) Object Parameters: Object parameters such as mass, location of center of mass with respect to a Universal or Base frame, and orientation of principal axes with respect to the Universal frame must be known. (2) Manipulator Parameters: Manipulator parameters include physical and geometric constraints such as mass and structure of the links, maximum joint rates, accelerations and torques, and the limits on the usable workspace. (3) Effectors grasp positions and orientations: Initial startup configuration of the effectors is made a task specification in this formulation. Various factors based on object parameters can be used to determine an optimal startup configuration of the effectors for the given task. The problem of determining an optimal startup configuration is not addressed.
PAGE 132
122 If the contact between the object and the effectors is not rigid, then the number of degrees of freedom between the object and an effector should be specified. In this chapter, we address only rigid contact. (4) Normal vectors at the point (or surface, depending on the type of gripper) of contact between an effector and the object: The normal vectors, expressed in the frame at center of mass (principal axes), define the surfaces of contact between the effectors and the object. A limit on the maximum value of the normal forces must be specified in order to prevent deformation of the object. (5) Start and destination position and orientation of the object: The start and destination object pose is given in terms of the object center of mass and principal axes with respect to the Universal frame. (6) Move parameters: Move parameters include the linear and rotational velocity of transfer, and initial and setdown acceleration of the center of mass. (7) Load sharing constraints: In load sharing constraints, one may specify the payload capabilities of the individual manipulators. This information is used in the force distribution algorithm to distribute forces such that the effectors with the heavier payload capability handle greater forces. Trajectory Specification for a MultiMani pulator Task A trajectory of the object is conveniently specified by describing the translational motion of the object center of mass and the rotation of the principal axes with respect to a universal frame. Since the positions of the end effectors holding the object are fixed with respect to the principal axes at any instant of time, i.e. rigid contact, the
PAGE 133
123 motion of each effector can be determined from the motion of the object center of mass and the principal axes. The translational and rotational motion of the object is specified by six polynomials, each a function of time. Three polynomials describe the translational motion of the object center of mass. The polynomials for the rotation of the principal axes describe the rotations about each one of the 3 axes of the universal frame. In order to reduce the degree of the polynomial for a large number of via points of the object, piecewise splined polynomials, continuous in position, velocity and acceleration, can be employed for storing the coefficients. Note that the polynomials can be discontinuous at any point in time. Forces And Torques on an Effector in a Closed MultiManipulator Chain In this section, we identify the forces acting at each effector during the motion of the object. The force and moment at the object center of mass is a composition of the forces and moments on the effectors holding the object. Since the objective of the load distribution is to distribute the force and moment at the center of mass into forces and moments on the effectors, we derive in this section the equations for the center of mass force and moment in terms of the object linear and rotational velocities and accelerations. Since the resolution of the force and moment at the center of mass is not unique, various criteria such as minimization of the input power must be employed for load distribution. The last subsection describes the criteria for dynamic load distribution in a multi manipulator task.
PAGE 134
124 Classification of Forces on an Effector in a ClosedLoop Forces on the tips of effectors (points of contact between object and effectors) in a closed loop multi manipulator system can be classified into the following types: (1) Normal tip reaction forces. The tip reaction forces keep the object from slipping and falling. Excessive tip reaction forces can deform the object. (2) Linear translational forces. These forces are responsible for the translational acceleration of the object. (3) Forces to counter gravitational force and forces due to friction at contact points. Frictional forces are ignored in this formul ation . (4) Forces arising due to the interactive coupling of the manipulator chains when the object is in motion. The interactive forces between sample points on the object trajectory are difficult to predict. The interactive forces are due to a lead or lag in each manipulator's motion on the trajectory between points. This lead or lag can be due to backlash or gear friction or slower response time of certain joints of a manipulator. A simple way to account for the unpredictable forces of interaction is to sense each end effector force by means of a wrist force sensor or any other force sensor, and add the interactive force to the force due to the load distribution at each sample point. Derivation of the Equations for the Force and Moment at the Object Center of Mass In this section, we derive the equations for the force and moment on the object at any instant of time. Let B denote a fixed frame of
PAGE 135
125 reference for the multi manipulator task. Let F_ be the translational force (independent of gravity, gravity effect can be compensated separately) at the center of mass of the object. The frame of reference for F is the base frame B. If v denotes the translational velocity Â— Â— cm vector of the center of mass, v_ the absolute acceleration of the center of mass in the base frame, and m the mass of the object, then the equation for the linear force F_ is F = mv (4.1) Let 1= Fxi+ \ ^ (4.2) and V L = i+ Vy j + v^ l<_ (4.3) where j_, j_, k_ are unit vectors in the directions of the three axes of the base frame. The absolute acceleration (v) of the center of mass of the object referred to the base frame is obtained from the linear velocity 1. rm Â» linear acceleration a , angular velocity to , angular cm Â— cm acceleration u and unit vector r_ (from the origin of the base frame to the center of mass as shown in Figure 4.1) as I = lcm^ iiicm'^ (^cm^il) ^'^^L (4.4)
PAGE 136
126 Figure 4.1 Translation and rotation of object about the base frame
PAGE 137
where 127 =v i+v j+v k (4.5) Â— cmxÂ— yÂ— zÂ— ^ ' oi ^ = 0) i+co j+u) k (4.6) Â— cmxÂ— yÂ— zÂ— V'/ and CO =a)i+a)j+ojk (4.7) Â— cmxÂ— yÂ— zÂ— \ Â• I Then the components of the trans! ational force at the center of mass are given as FÂ„ = m(v + v 0) V co+r w r co) (4.8) X ^ X z y y z z y y z' \ "^i F = m(v + v (D V (D+r u r u) (4.9) y ^y xz zx xz zx' ^ ' F, = m(v, + v CO V co+r co r co) (4.10) z ^zyxxyyxxy' ^ ' where V = (0 X Â£ (4.11) The moment at the center of mass ciue to a rotation of the body by an angular velocity co can be expressed in the principal axes frame of the object by Euler's equations. Let I^^, 1^^, I^^ denote the moments of inertia of the object about the principal axes. If M^, MÂ„ and M, are X y z the three components of the moment in the frame of the principal axes.
PAGE 138
128 and u' , to' are the angular velocity and the angular acceleration of the object in the principal axes frame, then Euler's equations give M = I oj' + (I I ) 0)' (o' (4.12) X XX X zz yy' y X M = I 0)' + (I I ) oj' 0)' (4.13) y yy y ^ XX zz' z y M = I 0)' + (I I ) u)' 0)' (4.14) z zz z ^ yy zz' X z Thus Eqns. (4.8) to (4.10) give the expressions for the force at the object center of mass in the base frame, and Eqns. (4.12) to (4.14) the equations for the moment in the frame of principal axes at the center of mass of the object. Factors in a Force Distribution Scheme The objective of the force distribution scheme is to generate a load distribution taking into account all determinate forces on the system. The various factors that effect load distribution are surmised in this section. Since the load distribution problem is underspecified, solution techniques for solving equations that are fewer in number than unknowns must be employed. If all equations are linear, and a linear objective function is formulated, then the well known techniques of Linear Programming can be used to solve the equations [McGhee and Orin 1976, Orin and Oh 1981]. A most obvious factor for minimization in the load distribution for a multimanipulator closed chain is the power required of each manipulator to drive its end effector for the prescribed load. It has been shown that for a universal serieswound motor, the motor power, to
PAGE 139
129 a first approximation, is directly proportional to the motor torque [McGhee and Orin 1976], Therefore, the minimization of joint torques leads to a reduction in motor power. The joint torques for a manipulator with no external force at its end effector are a function of the robot pose, joint rates and joint accelerations. A simple 3link planar manipulator (Figure 4.2) is used to show the dependence of the joint torques on the robot pose. The plot of Figure 4.3 shows the computed torque on the motor of joint 1 for a linear motion of the tip of the last link. The 2 plots are for the same move, except that the startup configuration is different as shown in Figure 4.3. The plots show that the magnitude of the torque changes significantly for different arm configurations along the same Cartesian move of the end effector. Thus, it is likely that certain manipulators in the closed chain may be in a configuration that yields greater forces in the required direction with smaller torque requirements than other mani pul ators . The load distribution on the object trajectory takes place at specific intervals of time, determined by the desired sampling rate on the trajectory. Between successive object poses (position and orientation), some effectors translate more than other effectors. Since the trajectory is known, the work done by each effector between the current and the succeeding points can be estimated for a unit force in the tangential direction of the end effector motion. If all effectors are to share the load equally, then one of the factors in force balance is the uniform distribution of the work done by the manipulators between successive sample points. In other words, the end effector translating more than another end effector between successive sample points should
PAGE 140
130 Figure 4.2 3link planar manipulator for simulation
PAGE 141
131 03 > O E c la Â•r(/) OJ c +J o SfO +> o s(U 3 E cn IB Â•rI/) 4C O) o o (> Q. s3 o It4J ilO M +J CO c Â•r!> o Â£ Â•Â•> (U scz O 4lÂ»Ol 3 XI CT $(> O 3 1Â— J=i CO Â• Â•* 0) scn Â•rÂ— U
PAGE 142
132 generally carry a smaller load. In order to determine the direction of the force component that performs useful work at the end effector, a coordinate frame called the "force frame" is defined from the space curve of the end effector trajectory for each effector. The force frames, defined in the next section, serve as body attached frames at each effector for the determination of the useful component of the end effector force (contributing to work). The force frames are also defined as the reference frames for load distribution. A constraint on the normal tip reaction forces must ensure that these forces are well within the specified bounds in order to prevent the deformation of the object. Other factors such as (1) physical manipulator limitations on torques and (2) load distribution constraints described in the secton on task and constraint description, also contribute as factors in forcemoment distribution. Definition of "Force" Frames The space curve trajectory of each effector indicates the direction of the useful forces acting at the effector. The forces performing useful work are always tangential to the end effector space curve. The forces in the normal and binormal directions of the effector space curve behave like constraint forces since ideally they should cancel. At any given point of the effector trajectory, we can construct an orthonormal coordinate frame with the zaxis along the space curve tangent, xaxis along the space curve normal and yaxis along the binormal direction of the space curve trajectory. This moving coordinate frame is called the force frame.
PAGE 143
133 We employ the force frames to express the force and the moment at each effector tip. Since in load distribution we seek the direction and magnitude of the force and moment on each effector, the fixed directions of the force frame can be used to express the load distribution constraints on each manipulator for a unit force and unit moment in the 3 directions of the force frame. As shown later in the chapter, working with the manipulator constraints in the force frame simplifies the load distribution problem, as compared to dealing with individual constraints on each joint of the manipulator. Derivation of the Force Frames from the End Effector Trajectories Let the object motion be described by the following 6 polynomial functions of time: (1) translation along xaxis of base frame: x(t) (2) translation along yaxis of base frame: y(t) (3) translation along zaxis of base frame: z(t) (4) rotation about xaxis of base frame: ij;(t) (yaw) (5) rotation about yaxis of base frame: e(t) (pitch) (6) rotation about zaxis of base frame: (()(t) (roll) The six polynomials completely specify the motion of the object by describing the translation of the origin of principal axes (center of mass) and the rotation of the principal axes frame about the base frame. The choice of the degree of the polynomial depends upon the desired motion characteristics. Typically, quartic polynomials provide continuity in acceleration and are a good choice for representing motion where acceleration continuity is desirable. For a continuum of points, cubic splined polynomials also provide continuity in acceleration.
PAGE 144
134 Consider the instantaneous motion of effector "n", attached to the object. Let r be the radial vector from the center of mass to the Â— n effector n ( Â£_ is a vector in the base frame). The tangent to the space curve of effector "n" is determined from two velocity components: Â• Â• Â• (1) the transl ational velocity, given by [x(t), y(t), z(t)] , and (2) the rotational velocity component at the center of mass, given by Ci(t), e(t), J(t)] X r ^ . Let Ln = ^ni" '2n i " ^3n ^ (^'^^^ Let _v j^i^ denote the rotational velocity component. Then, V [^(t). 0(t), <.(t)] X r ^ ^(^3n Â®^2n ' * ;3n ' ^ ir^, I r^, e)] (4.16) If _ is the resultant transl ational velocity and ^ is the unit vector in the direction of v_ ^ (also the tangent to the space curve of effector "n"), then Vl I L = t:^. ^] + ^i^3n Â® ^2n ^^'/^In * ; ^3n ' . ^'2n \% = ^(^^ ^3n ^2n ^ 'in \^3n '^]' (z+ r^^ ir^^ 6)]
PAGE 145
135 where The normal to the space curve of effector "n" is given by the rate of change of the tangent with respect to the space curve. The direction of normal is always towards the center of curvature of the space curve. dTj_/ds = (dT_/dt) (dt/ds) = (l/v^) (dT^/dt) = < N L (4.19) where k is the radius of curvature (always chosen positive) and _N ^ is the normal to the space curve. < N L = (1/Vl)[(1/Vl) {(x" + r3^ 6"r^^ f) , (y" + r^^ f r^J') (z.r^^rr^^e)} ){(x+ r3^ 0^r^^ ;).(;+ r^^ \ r^^ h, ^2n ^ ''in ^" ^in ^^(^^ ^3n 'Zn
PAGE 146
136 . \ ^in * '^Sn '2n ' ^^^^ ''in ^3n ^in * " ^n " 'Zn '2n 'lu '2n '^in '^^^ ^^'^'^^ The magnitude of the vector in Eqn. (4.20) is < . Eqns. (4.17) and (4.20) give the vector tangent and normal to the space curve of effector "n". The binormal is the cross product of the tangent and the normal vectors. The direction of the end effector space curve normal becomes undefined when the trajectory is linear. Let us consider a linear motion of the object with no rotation. In this case, the tangential vectors to the space curves of all effectors and the center of mass are parallel. The direction of the normal (and binormal) vector is arbitrary, as long as the normal is perpendicular to the tangential vector. Since the tangential vectors of the effectors and the center of mass space curves are parallel, we propose the following technique to determine a common normal (and binormal) vector for the linear space curves. The technique is to rotate the zaxis of the principal axes frame into the tangential vector at the center of mass. The x and yaxes of the rotated principal axes frame yield the normal and binormal vectors of the linear space curves, p Let z_ denote the unit zvector of the principal axes frame (the superscript P shows that the frame of reference is the principal axes
PAGE 147
137 p frame). If t_ denotes the unit tangential vector (along the linear p space curve) at the center of mass, then an axis of rotation, k^, to rotate the zaxis of the principal axes frame into the tangential vector is obtained as k = ^ z X ""t L (4.21) p The angle of rotation about this axis is the angle between z_ and p t_ ^ . Let Y denote the angle of rotation. Then, cos T = z . ^t^ (4.22) sin Y = I ""z x ''t L I (4.23) Y = ATAN2 ( sin y, cos y) (4.24) where ATAN2 is the inverse tangent function that returns the angle and quadrant. The normal and bi normal vectors to the linear space curve can now be derived from the rotation matrix R as follows: = ^[Rp] ^[Rot(^, y)] (4.25) where l Rp] is the 3x3 rotation matrix with the principal axes vectors expressed in the base frame, and Rot (_k, y) is the rotation matrix derived in [Paul 1981].
PAGE 148
138 Thus, the above technique can be employed to obtain a direction for the normal and bi normal vectors when the space curve of an end effector or effectors, or the center of mass, or both the effectors and the center of mass (linear motion of the object) is linear. We now describe two algorithms for forcemoment distribution in the next section. Algorithms for ForceMoment Distribution In this section, two methods are described for forcemoment distribution in a closedloop multi manipulator system. The first method simplifies the load distribution problem by decoupling the forces and moments between successive points on the object trajectory. The decoupling of the forces and moments splits the load distribution problem into 2 independent components. Since linear programming techniques are employed to solve the underspeci f ied problem of force and moment distribution, the decoupling speeds the solution since the number of equations and the initial tableau size are smaller. The decoupled load distribution problem is set up such that only the force distribution requires linear programming. The moment distribution follows the force distribution and the solution for moments does not require linear programming. The error due to decoupling of the object translational and rotational forces is proportional to the linear and angular velocities. Thus, the decoupled forcemoment distribution is a feasible approach for slow object motions. If the error due to the decoupling of the translational and rotational motions is not tolerable, then a combined forcemoment distribution must be done at each sample point. The setup of the force
PAGE 149
139 and moment equations in the form of a linear programming problem is described in later sections. For any linear programming problem, an objective function must be defined. The solution given by linear programming is highly dependent on the objective function defined for the problem. The next section defines a parameter called the Average Incremental Work for each manipulator (AIW) that is employed in the construction of the objective function. Definition of the Average Incremental Work per Unit Force and Moment In this section, we define a factor called the Average Incremental Work (AIW) for the manipulator motors when a unit external force or a moment is applied at the end effector. The AIW provides a measure of the average increase in the motor power of the manipulator to counter an external force or moment at the end effector. The AIWs are computed at each "quasistatic" [Vukobratovic and Stokic 1983] sample period on the object trajectory. Thus, the AIWs are determined at each sample instant where a load distribution is required on the object trajectory. Since the reference frames for the load distribution on the object are the force frames, the AIW factors are defined for unit forces and unit moments in each of the 3 directions of the effector force frames for each arm. Thus, a total of 6 AIW factors, 3 for unit forces and 3 for unit moments, are defined for each manipulator. The AIWs will be determined from an openloop dynamic analysis of a manipulator chain. Initially, the incremental joint torques from no load to a unit force or moment are determined. Then, a function relating the motor input power to torque and other joint parameters is used for determining the incremental power. Since a criterion for load distribution is to reduce power, the AIW factor is defined as the average incremental power
PAGE 150
140 required of each motor for a manipulator. Note that the AIWs are nonlinear functions of the joint torques. This nonlinearity does not affect their use in the objective function of the linear programming problem, since the AIWs will be constant numbers determined apriori, and the objective function does not have joint torques as variables. Let us consider a manipulator chain whose joints are powered by series wound DC motors. To a first approximation, it has been shown that the input power to a series wound DC motor is directly proportional to the joint torque [Orin and Oh 1981, McGhee and Orin 1976]. The AIW factors for this case are defined as the average incremental joint torques for a unit force or moment in the 3 directions of the force frame. Let [t^, tp,...,tg] be the joint torques with no external force or moment on the end effector. Let [t^^p^^, t2pxÂ» Â•Â•Â• Â» ^SFx' joint torques for a unit force in the xdirection of the end effector force frame. Then AIW^p^^ is defined as Note that the joint torques [t^, ... ,tg] and [t^p^^, ... .tgp^] can be determined by an "inverse" dynamic analysis. The data required (other than the parametric data for the manipulator) are external forces/moments on the effector, the current joint positions, velocities and accelerations. This data is easily derived from the end effector trajectory [Whitney 1969, Luh et al . 1980a]. The procedure for the inverse dynamic analysis can be either one of 2 well known approaches: NewtonEuler [Luh et al . 1980a] or Lagrangian [Hollerbach 1979].
PAGE 151
141 By the definition of the AIW factors, the higher the value of a force or moment AIW, the smaller should be the respective force or moment in that direction of the force frame. An additional work factor, described in the section on factors in a force distribution scheme, is added to the force AIWs in the tangential direction of the end effector space curves. This factor is the work done by each effector between successive sample points on its trajectory. Since the load distribution should provide a uniform distribution of the load between sample points, the work done by each effector between sample points given by the line integral of the tangential force, is added to the force AIW factor in that direction. Since the zaxis of the force frame denotes the tangential direction, the AIW^p^ is modified as AIW.p^ = k^ AIW.p^ + k^l /f dz I (4.27) where k^ and k2 are weighting factors. The integral in Eqn. (4.27) is computed for a unit force along the tangent _f from the knowledge of the end effector space curve between the sample points. A Decoupled ForceMoment Balance In order to simplify forcemoment distribution, the translation and rotation of the object can be decoupled for each incremental motion of the system. The decoupled motion of the object center of mass becomes a pure translation followed by a rotation about an axis. During translation, all effector trajectories will be parallel to the center of mass trajectory. Therefore, the force frames of all effectors are
PAGE 152
142 parallel during the translation phase of the motion. The force at the center of mass is simply the sum of the gravitational force (m_Â£_) and the linear translational force (m^) . Â£ = m (a_+ Â£) (4.28) The effect of the angular velocity and angular acceleration on the forces at the effectors is ignored. The error "e" in the translational force on an effector due to ignoring the angular velocity effect can be derived as e = m. (oj ^Â„ X (ti) X r) + oj x r) (4.29) Â— Â— cm Â— cm Â— Â— cm Â— where oo , oo are the angular velocity and angular acceleration, Â— cm Â— cm and r_ is position vector from the origin of the base frame to the center of mass. If the rotational velocity and rotational acceleration are small, then Â£ given by Eqn. (4.29) can be ignored, and the simplified load distribution of this section applies. .The force at the center of mass in base coordinates, given by Eqn. (4.28) can be resolved into 3 components along the vectors of the force frame at the center of mass. Let {^q^^, ^cmy '^cmz^ denote the 3 components of the center of mass translational force along the axes of the force frame. ('^cmx' ^my f^cmz)' = '^^B ^' (^'^O)
PAGE 153
143 where '^Rg is a 3x3 rotation matrix from the base frame to the center of mass force frame (see [Paul 1981] for the derivation of static force transformations between coordinate frames). Let (f^^, fjy, f^^), (f2x, f2y' ^2z'>' "' '^^nx' '''ny' 'Â•"nz^ represent the forces transformed from the base frame to the force frames of the n end effectors. Since all force frames are parallel for a pure translation, the following 3 force equality equations are true. ^Ix ^ ^2x "' ^ ^nx ''cmx (4.31) f, + fo + ... + f = F ly 2y ny cmy (4.32) f, + fo + ... + f = F Iz 2z nz cmz (4.33) Note that we have 3 equations and 3 * n unknowns. Therefore, for n > 1, there is no unique solution to the forces on the end effectors. Since forces and moments are decoupled in this section, we initially address force distribution and then moment distribution. The force and moment distribution equations will be set up as two separate problems. In order to decouple force and moment distribution, we impose the constraint on the forces at the end effectors that their resultant moment at the center of mass is zero. Thus, if r_ ^ denotes the vector from the origin of the force frame at the center of mass to the effector k ( _r . referenced to the force frame at the center of mass), then (4.34)
PAGE 154
144 where ^"'Rj^ is a 3x3 rotation matrix that transforms a force in the force frame of effector k to the corresponding force in the frame at the center of mass. Since the force frames for the load distribution of cm this section are all parallel, the rotation matrix Rj^ is an identity matrix. During force distribution, other than minimizing the power input, inequality constraints on the maximum normal force on the object must be introduced to prevent deformation of the object. Also, the magnitude of the joint torques must be checked against a maximum specified value for the motor. The constraints on the joint torque are ignored in this section because (1) the system angular velocity and angular acceleration are low, and (2) force distribution according to the AIW factors leads the system towards smaller torques. The constraints on the forces normal to the object at the end effector contact points, however, cannot be ignored. Let f^jmax maximum specified magnitude of the object surface normal force for effector "i". Let fj^^ be the magnitude of the surface normal force due to the force distribution. Then, I^Ni I ' 'mm. The magnitude function in Eqn. (4.35) is a nonlinear operator, and cannot be used in the equations for linear programming. In order to "linearize" Eqn. (4.35), an additional 2 variables are added [Llewellyn 1969, Orin 1979] such that ^Ni = 4 (^36)
PAGE 155
145 and ^'m+ ^Kr < (4.37) The variables f^^ and fj^^^ are both positive. The inequality (4.37) can be transformed to a linear equation by the addition of a "slack" variable x^as follows f,,. + f,,. + x. = f... (4.38) Ni Ni 1 Nimax ' Thus 2*n additional equations (Eqns. (4.36), (4.38)) are added to the force distribution problem. The objective function is formulated such that the overall system power is reduced. The AIW^ , defined in the preceding section, estimates the relative increase in the power supplied to manipulator "i" in response to an external force. If the AIW is large for a manipulator, then the load distribution should seek other manipulators that require less power, but allow the creation of the desired motion. Thus, the external force at an end effector of a manipulator should be inversely proportional to the respective AIW factor. We define a set of factors AAA (o. , a. , a. ) for i = 1 to n as follows: ^ ix' iy' m' A (1/AIW.^) (4.39) (1/AIW.y) (4.40) A (1/AIW.^) (4.41) a. = IX A a. = A a. = 1 z
PAGE 156
146 For reduction in power, the forces should be directly proportional A A to the set of (a. , a. , a. ) . At this stage, we introduce user defined 1 X 1 y 1 z A constraints on the manipulator payloads (in Kilograms) to modify "a" as follows. Let the payload ratio of the n manipulators in the chain be given as 1^: l^: I3 ... : 1^ (4.42) Each set of (a. , a. , a. ) is modified according to the weighted IX 1 y 1 z average of the payload ratios. Thus, for effector "i", n .) (4.43) (a. , a. , a. ) = (1./ El.)(a. , a. , a. ^ ix' iy' m' ^ 1 ._. j'^ ix' iy' 1 Z' Thus Eqn. (4.43) modifies the a factors such that each set of a factors for a manipulator reflect the prorated share of the manipulator's payload. Without the force constraints of this section ( (1) constraint on the magnitude of the effector surface normal forces, and (2) constraint on the effector forces that the moment at the center of mass is zero), the force at each effector should equal a proportion of the force at the center of mass determined by a ratio of the a factors as follows: fz= W Â«iz/ Â°jz) (^44) where fl^ is the force in the zdi recti on of the effector force frame expressed in the center of mass force frame and F is the force in cmz the zdirection of the center of mass force frame. For the linear
PAGE 157
147 programming solution, we formulate the objective function so as to pairwise cancel the end effector force and the corresponding force obtained by the distribution of the center of mass force strictly in accordance with the a factors. If f^^ denotes the zforce component of the ith effector, then a factor Aj^ defined as follows: Z a. j=l The objective is to minimize A^^ , because when A^^ = 0, the force at the effector is exactly in the desired ratio of the a's. The objective function is therefore defined as MINIMIZE [ _Z^[ A.J + I A.^1 + I A.J)] (4.46) Thus, the linear programming setup for force distribution has (2n + 3) equations with 6*n unknowns. The objective function of Eqn. (4.46) tends to reduce the input power and also attempts to distribute the forces according to the specified pay load capabilites of the manipulators. Distribution of Moments The moment at the center of mass is due the rotation of the object about an axis in space. Let M^.^^^, M^.^^, M^.^^^ denote the 3 components of the moment at the center of mass referred to the force frame. The equations for the moment components in the force frame are
PAGE 158
148 cmx cmy cmz Fcnir I 0)' + (I XX X ^ zz I li' + {I yy y xx I 0)' + (I zz z ^ yy I ) Co' 0)' yy y z I ) 0)' UJ' zz x z I ) 0)' 0)' XX ' x y (4.47) where Rp is the 3x3 transformation matrix from the principal axes frame to the center of mass force frame and w' , u' are the angular velocity and acceleration of the object in the principal axes frame. Let (m^x. m^^y, "^Iz^' ^^2x> ^2y' "^2z)Â» Â•Â•Â• ('"nx* %y> "^nz^ represent the moments in the force frame directions of the n effectors. Since all force frames are parallel. m, + + . . . + m = M Ix 2x nx cmx (4.48) m, + + . . . + m = M ly 2y ny cmy (4.49) m, + m^ + . . . + m = M Iz 2z nz cmz (4.50) The moment distribution from Eqns. (4.48) (4.50) is similar in principle to the force distribution. The moments in the effector force frames should be inversely proportional to the AIW factors determined for unit moments in each direction of the force frame. We define a set of factors (B^j^, e^y, 3^^) ^Â° " follows: e. = 1/(AIW) . IX ^ 'mix (4.51) e, = i/(Aiw) . iy ^ 'miy (4.52)
PAGE 159
149 6. = 1/(AIW) . (4.53) Since no further constraints are imposed on moment distribution, the solution to Eqns. (4.48) (4.50) can be efficiently determined by a direct weighted average ratio of the factors given by Eqns. (4.51) (4.53). Thus, m. n 2 6, cmx j = l JX m. iy n j = l cmy jy m. = M iz n cmz ^ 3, j = l JZ (4.54) (4.55) (4.56) A General ForceMoment Distribution Scheme The decoupled forcemoment distribution scheme of the previous section provided an approximate force moment balance for slow motions of the closedloop system. In this section, we address the general forcemoment distribution with inequality constraints on the maximum normal force and the maximum joint torques of each manipulator. As stated before, the frictional forces are ignored. This section is organized as follows. Initially, we derive the equality equations between the center of mass force and moment and the
PAGE 160
150 end effectors forces and moments. The inequality constraints on the maximum normal force and the maximum joint torques are described next. Finally, the objective function for the solution of the linear equations is derived. Force and Moment Equality Equations The force components at the center of mass in the base frame are (Eqns. (4.8)(4.10)) F =m. (v + v oj V oj+r w r a))' (4.57) X ^xzyyzzyyz' F =m. (v + v 0) V (D+r ur u) (4.58) y ^yxzzxxzzx' ^ ' F =m. (v+v 0) V to+r u r u) (4.59) z wyxxyyxxy' ^ ' ys A A where (v , v , v ) are defined by Eqn. (4.11), (co , o) , u) ) are the X y z X y z Â• Â• Â• components of the angular velocity and (v , v , v ) are the components X y z of the linear acceleration, all vectors referenced to the base frame. If f_ . , f_ . , jf ^denote the forces in the 3 directions of the force IX 1 y 1 z frame of effector i, then the 3 equality equations for the forces can be derived from the following vector equation: n F = I (f . + f , + f . ) (4.60) Â— cm 1 Â— IX Â— iy Â— m' ' where ^"^R^is a 3 X 3 rotation matrix that transforms a vector in the force frame of effector "i" to the center of mass force frame. The moment equations relating the moments at the center of mass to the moments on the effectors is derived as
PAGE 161
151 (4.61) (4.62) zcomp (4.63) where (M , M , M ) denotes the moment due to the object rotation and \ x' y' z' this moment in the force frame at the center of mass is given by Eqn. (4.47). The moment equality equations are now given by the following vector equation: Inequality Constraints on Normal Forces and Joint Torques The inequality constraints on the normal forces at the n effectors are expressed as Eqns. (4.35) and (4.37). The conversion of the inequality constraints to linear equations, as in Eqn. (4.37), introduces a total of n "slack" variables in the linear programming probl em. Since there is an upper and lower bound on the joint torques, inequality constraints on the magnitude of the torques must also be included. In order to check for a torque bound, given an external force at tne effector, we employ the values of the openloop torques per unit force and moment which are derived for the determination of the AIWs. '^^ ('^ivi* "^iwi' ""iTi) denote the open loop torques expressed in the ' xj lyj 1 zj base coordinates, of joint j per unit force and unit moment in the x, y n Z i=l cm . (m . + m . + m . ) 1 Â— IX Â— iy Â— ^z' (4.64)
PAGE 162
152 and z directions of the force frame of effector i. Thus, for a given force and torque at an effector, an estimate of the torque requirement from the joints is obtained by the linear relationship between the forces and moments at the end effectors and the joint torques. (t.) ^ = (a. f . ) T. . + (a. f . ) T. . + [a. f. ) t. . ^ j'est ^ IX ix' 1XJ ^ iy iy' lyj ^ iz iz' lyj + (u. m. ) T . . + (y. m. ) T . . + (y. m. ) t . . cc\ ^ IX ix' mixj ^ iy ly' miyj ^ iz iz' mizj (4.65) where (a.^, a.^, a.^) and (y.^, y.^, y.^) are porportional ity constants . I (t.) < (x.) (4.66) ' ^ J 'est' ^ J 'max ^ ' The transformation of Eqn. (4.66) into linear equations (as for in Eqns. (4.36), (4.38)) introduces an additional 2*n*m equations, where n is the number of manipulators and m is the number of joints per manipulator. Formulation of the Objective Function The formulation of the objective function is identical in principle to the objective function of the previous section. The idea is to determine a set of values for the effector forces and moments such that these values are as close as possible to the ratios of the as and 3s defined by Eqns. (4.43) and Eqns. (4.51) to (4.53). Denoting the effector forces as (f^^, f^^, f^^) Â•Â• (^nx* ^nyÂ» ^nz^' effector moments as (m^^, m^^, m^^) ... (m^^, m^^, m^^)* ^ifx ^imx defined as
PAGE 163
153 A. . ifx = [ A. imx f . 1 X "ix n n E f . j=l J' Z a. j = l m. 1 X ^Â•x ^ n n ' E m. j=l Z g., (4.67) (4.68) Similarly, A^^^, A^^^, A^^^ and A^^^^ can be defined. The objective function is therefore n MINIMIZE [ /^(lA.^J + lA.^^I + A.^^ i = l "J' (4.69) n + E ( I A. I + A. I + A. I )] ^' imx' imy' imz" Thus, the general forcemoment distribution has a total of [2n(m<l) + 6] equations, where n is the number of manipulators and m the number of links per manipulator, (6 forcemoment equality equations, 2n equations for the normal force limit inequality, 2nm equations for the maximum torque inequality) with [3n(m + 1) + 6n] variables. An Illustrative Example In this section, we describe the simulation results of the load distribution schemes of the preceding section on a simple 2 manipulator closedloop task. The manipulators are 3link 3degrees of freedom planar manipulators (Figure 4.2). Each link of the planar manipulator is assumed 2 m long. All links are cylindrical with masses of 1 Kg.
PAGE 164
. 154 each. The task set up is shown in Figure 4.4. F^ and F2 denote the "base" frames of the 2 manipulators. The object is a uniform rigid cube with 1 m side length. F^^^^ denotes the object frame at the center of mass. The objective of the simulated task is to translate and rotate the object from to P3 (Figure 4.4). A constant translational acceleration of 0.25 m/sec^ is assumed. It is also assumed that the start up velocity of the object at P^^ is 1 m/sec and the destination velocity at P3 (along P2P3) is also 1 m/sec. Thus, the task is to transform the object at P^ with a linear velocity of 1 m/sec. (in the direction P^^ P2) to P3 with a uniform acceleration of 0.25 m/sec'^, and rotate the object about the z_ axis a total of ((> = n/3 radians between P^ and P3. The move time for the segment P1P3 is calculated [Taylor 1979] as 2t = I V Â„ v J / aÂ„^^ = K 3/4 + 1/4 I / 0.25 = 2 sees. (4.70) The object is rotated II/3 radians between P^ and P3. The rotational velocity (about the zaxis) is 0)^ = /2T = n/6 rad/sec (4.71) The center of mass trajectory between P^^ and P3 can be expressed as a function of time as 0 < t < 2t (4.72)
PAGE 165
Figure 4.4 Two manipulator task for simulation
PAGE 166
156 where P_ ^ is the start point, = linear velocity (1 m/sec) ^ = unit vector in the direction P2 a = (1 2 " l^/^"" The simulation program for testing the load distribution schemes of this chapter comprises the following 5 Pascal program modules: (1) kinematics of the 3link planar manipulator, the forward and reverse solutions, (2) dynamics of the 3link planar manipulator, routines for reverse velocity and acceleration and a routine for determination of the joint torques, (3) a module for path generation, (4) a module for solving linear equations employing the Simplex method [Llewellyn 1969], and (5) a module for the determination of the load distribution factors and setting up the load distribution equations. The object trajectory is sampled every 50 ms. At each sample period, the joint torques are computed from the trajectory data and the load distribution at the end effectors. For simplicity, no constraints are imposed on the maximum joint torques and the end effector normal forces. The plots of Figures 4.5, 4.6 and 4.7 show the joint torques versus time for one of the manipulators in the closedloop task. In order to illustrate the reduction in the overall system torque (power), we plot the sum of the magnitudes of the joint torques of both manipulators for a load distribution employing the a factors of Eqns. (4.39)(4.41) , and a load distribution by assuming equal loads at both end effectors for all configurations. The plots are shown in Figure 4.8. We notice that the load distribution for this example reduces the overall system torque over that of an equal load distribution. A plot of one of the
PAGE 167
157
PAGE 168
153 c o CM P s_ O 3 Â•"3 4Ll_ O M0) O cr J> o O 1Â— IJ2 m
PAGE 169
159
PAGE 170
160 Â£ %. >> t> c (/> Â•ro D o 4O O t/1 T3 Â•o sz 3 (0 M Â•rc C o cn Â•r
PAGE 171
161 AIW factors, namely the force AIW factor in the xdirection of the force frame of each effector in Figure 4.9 shows the change in AIW^ versus time for the simulated move.
PAGE 172
162
PAGE 173
CHAPTER V CONCLUSION This dissertation provides new solutions for online tracking of end effector trajectory, "feasible" and accurate motion planning by creation of a "rotation space" and independent deterini nation of rotational motions, and force coordination in a multirobot task. The main contributions are surmised as follows: (1) A new model is developed for joint interpolated motion. The model employs cubic polynomials for startup and setdown. A novel feature of the model is the constraint on coordination of the pacing and nonpacing joints during startup, cruise and setdown. This constraint reduces the startup jerk in the nonpacing joints by clipping the acceleration during the startup and setdown without affecting the total move time. The performance evaluation of the joint motion model by simulation and experimentation on an industrial robot demonstrates the superiority of the proposed model to the current model. (2) We developed an online trajectory approximation technique for the joint trajectories during a constrained end effector motion. We derive a factor for the determination of the ideal number of lookahead points on the end effector trajectory. This factor, called the system memory error, is indicative of a certain level of performance in the trajectory. The primary attributes of the online cubic splines are tracking accuracy and continuity in at least the first derivative. The testing of the online cubic splines by simulation and actual 163
PAGE 174
164 experimentation on an industrial robot showed excellent results in terms of tracking accuracy and smooth motion. (3) We define and develop independent rotational planning of end effector motions. A "rotation space" is defined for spherical wrist manipulators by the manipulator degrees of freedom from the wrist to the effector. We proved that the configurationfree estimate of the maximum tool rotational acceleration in rotation space is the full load acceleration of the slowest joint in the rotation space. (4) A method for independent rotational planning of both unconstrained and constrained tool rotations is derived. We describe a technique for unconstrained pointtopoint rotational trajectories employing the maximum constraints on the joint accelerations. A simulation example where the existent translationdominant techniques fail to produce a "feasible" motion clearly illustrates the applicability of independent rotational planning for producing smooth and feasible motions. (5) Two linear models are developed for the active distribution of the object load among the manipulators in the coordination of a closed chain multimanipulator task. A set of moving coordinate frames, called force frames, are defined at each effector and the object center of mass as reference frames for load distribution. It is shown that the force frames simplify the formulation of constraints on the load distribution problem. A linear model for load distribution is formulated such that (a) the system power is reduced, (b) the joint torques are limited to their respective maximum values, (c) the magnitude of each surface normal force on the object is constrained to a maximum value, and (d) the load distribution reflects the payload capabi 1 itites of the
PAGE 175
165 individual manipulators. A simulation of the proposed load distribution model demonstrates the reduction in the system power at each sample instant. The work in this dissertation is one step towards greater cooperation in multirobot tasks. Most of the results presented in this dissertation have been verified either by simulation or experimentation, or both. The linear models of Chapter IV for the load distribution in a closed chain can be refined to include (1) prediction and compensation of the interactive forces and moments in the load distribution, (2) inclusion of frictional forces, and (3) allowing degrees of freedom between the object and the effectors.
PAGE 176
REFERENCES Ahlberg, J. H., Nilson, E. N., and Walsh, J. L., The Theory of Splines and Their Applications , Academic Press, New York, 1967. Anderson, T. R., and Paul, R. C, "High speed coordinated control of industrial robots," Proc. 9th Int'l Symposium on Industrial Robots, SME, pp. 449455, Mar. 1979. Bedina, R. A. 0., "Academic research on the Oli vettisigma system applications," Proc. of the 7th ISIR, pp 665674, 1977. Bejczy, A. K., "Robot arm dynamics and control," Technical Memo. No. 33669, Jet Propulsion Lab, Feb. 1974. Bollinger, J., and Duffie, N., "Computer algorithms for high speed continuous path robot manipulation," Ann. CIRP, Vol. 28, No. 1, pp. 391395, 1979. Clenshaw, C. W., and Negus, B., "The cubic Xspline and its application to interpolation," J. Inst. Math. Appl., Vol. 22, pp. 109112, 1978. DeBoor, C, A Practical Guide to Splines , Springer Verlag, New York, 1978. Devanit, J., and Hartenburg, R. S., "A kinematic notation for lower pair mechanisms based on matrices," ASME Journ. Appl. Mech., pp. 215221, June 1955. Duffy, J., Analysis of Mechanisms and Robot Manipulators , John Wiley and Sons, New York, 1981. Featherstone , R., "Position and velocity transformations between robot and effector coordinates and joint angles," Int'l J. Robotics Res., Vol. 2, No. 2, pp. 3545, 1983. Finkel, R. A., Constructing and debugging manipulator programs, Ph.D. Dissertation, Stanford University, 1976. Giralt, G., Sobek, R., and Chatila, R., "A multilevel planning navigation system for a mobile robot," Proc. 6th Int'l Joint Conf. Artificial Intelligence, pp. 309321, Aug. 1979. Gleason, G. J., and Agin, G. J., "A modular vision system for seniorcontrolled manipulation and inspection," Proc. 9th Int'l Symposium on Industrial Robots, SME, pp. 5770, Mar. 1979. 166
PAGE 177
167 Golla, D. F., Gang, S. C, and Hughes, P. C, "Linear statefeedback control of manipulators," Mech. and Machine Theory, Vol. 16, pp. 93103, 1981. Hemami , H., and Wyman, B. F., "Modeling and control of constrained dynamic systems with application to biped locomotion in the frontal plane," IEEE Trans, on Automatic Control, Vol. AC24, No. 4, pp. 526535, 1979. Hollerbach, J. M., "A recursive lagrangian formulation of manipulator dynamics and a comparative study," AI Lab. Memo. No. 533, M.I.T., 1979. Hollerbach, J. M., and Sahar, G., "Wristpartitioned inverse kinematic accelerations and manipulator dynamics," Int. J. Robotics Res., Vol. 2, No. 4, pp. 6176, 1983. Inoue, H., "Force feedback in precise assembly tasks," AI Lab. Memo. No. AIM308, MIT, 1974. Ishida, T., "Force control in coordination of two arms," Fifth Int'l Joint Conf. on Artificial Intelligence, M.I.T., pp. 717721, 1977. Johnson, R., Numerical Analysis , Addison Wesley, Reading, Massachussetts, 1977. Lewis, R. A., "Autonomous manipulation on a robot: summary of manipulator software functions," JPL, Cal . Inst, of Tech., TM No. 33679, 1974. Lin, C. S., and Chang, P. R., "Joint trajectories of mechanical manipulators for cartesian path approximation," IEEE Trans, on Sys., Man, and Cybernetics, Vol. SMC13, No. 6, pp. 10941102, Dec. 1983. Lipkin, H., and Duffy, J., "Analysis of industrial robots via theory of screws," 12th Int'l Symposium on Industrial Robots, June 1982. Llewellyn, R., Linear Programming , Holt, Rinehart and Winston, New York, 1969. LozanoPerez, R., and Wesley, M. A., "An algorithm for planning collisionfree paths amongst polyhedral obstacles," Research Report 7171, IBM T. J. Watson Research Center, Oct. 1979. LozanoPerez, T., "Automatic planning of manipulator transfer movements," IEEE Trans, on Sys., Man, and Cyb., Vol. 11, No. 10, pp. 681689, 1981. Luh, J. Y. S., and Lin, C. S., "Optimum path planning for mechanical manipulators," Trans, of the ASME, Vol. 102, pp. 142151, June 1981.
PAGE 178
168 Luh, J. Y. S., Lin, C. S., and Chang, P. R., "Formulation and optimization of cubic polynomial joint trajectories for industrial robots," IEEE Trans, on Automatic Control, Vol. AC28, No. 12, pp. 10661074, Dec. 1983. Luh, J. Y. S., Walker, M. W., and Paul, R. C, "Online computational scheme for mechanical manipulators," Journ. of Dynamic Systems, Measurement, Control, Vol. 102, pp. 6976, 1980a. Luh, J. Y. S., Walker, M. W., and Paul, R. C, "Resolved acceleration control of mechanical manipulators," IEEE Trans. Automatic Control, Vol. 25, Mo. 3, pp. 468474, 1980b. McGhee, R. B., and Orin, D. E., "A mathematical programming approach to control of joint positions and torques in legged locomotion systems," Proc. 2nd Int'l CISMIFToMM Symposium, pp. 225232, 1976. Nakano, E., Ozaki , S., Ishida, T., and Kato, I., "Cooperational control of a pair of anthropomorphous manipulators "MELARM"," Proc. of the 4th Int'l Symposium on Industrial Robots, pp. 251260, 1974. Orin, D. E., and Oh, S. Y., "Control of force distribution in robotic mechanisms containing closed kinematic chains," Trans, of the ASME, Vol. 102, pp. 134141, 1981. Paul, R. C, "Modeling, trajectory calculation and servoing of a computer controlled arm," Ph.D. dissertation, Stanford University, California, 1972. Paul, R. C, "Manipulator cartesian path control," IEEE Trans, on Sys., Man, and Cybernetics, SMC9, No. 11, pp. 702711, 1979. Paul, R. C, Robot Manipulators : Mathematics, Programming and Control , MIT Press, Cambridge, Massachussetts , 1981. Paul, R. C, and Shimano, B., "Compliance and control," Proc. 1976 Joint Automatic Control Conf., pp. 694699, 1976. Raibert, M. H., and Craig, J. J., "Hybrid position/force control of manipulators," Journ. of Dynamic Systems, Meas. and Control, Vol. 102, pp. 126133, 1981. Raibert, M. H., and Tanner, J. E., "Design and implementation of a VLSI tactile sensing computer," Int'l Journal of Robotics Research, Vol. 1, No. 3, pp. 318, 1982. Seireg, A., and Arvikar, R. J., "The prediction of muscular load sharing and joint forces in the lower extremeties during walking," Journ. of Biomechanics, Vol. 8, pp. 89102, 1975. Stute, G., and Erne, H., "The control design of an industrial robot with advanced tactile sensitivity," Proc. 9th Int'l Symposium on Industrial Robots, SME, pp. 519527, Mar. 1979.
PAGE 179
169 Taguchi , K., Ikeda, K., and Matsumoto, S., "Fourlegged walking machine," Proc. 2nd Int'l CISMIFToMM Symposium, pp. 162171, Sept. 1976. Takeyasu, K. A. 0., "An approach to the integrated intelligent robot with multiple sensory feedback: construction and control functions," Proc. of the 7th ISIR, pp. 293306, 1977. Taylor, R. H., "Planning and execution of straightline manipulator trajectories," IBM Journ. of Res. and Dev., No. 23, pp. 424436, 1979. Udupa, S. M., "Collision detection and avoidance in computer controlled manipulators," Ph.D. dissertation. Dept. of Electrical Engineering, Cal . Inst, of Technology, 1977. Vukobratovic, M. and Stokic, D., "Is dynamic control needed in robotic systems, and, if so, to what extent?," Int'l. J. Robotics Res., Vol. 2, No. 2, pp. 1834, 1983. Warnecke, H. J., and Schraft, R. D., Industrial Robots, Applications Experience , I.F.S. Publications Ltd., Bedford, England, 1979. Whitney, D. E., "Resolved motion rate control of manipulators and human prostheses," IEEE Trans, on Man, Mach., Sys., Vol. MMS10, pp. 4753, June 1969. Whitney, D. E., "The mathematics of coordinated control of prostheses and manipulators," J. Dynamic Systems, Measurement, Control, pp. 303309, Dec. 1972. Williams, R. J., and Seireg, A., "Interactive modeling and analysis of open or closed loop dynamic systems with redundant actuators," ASME Journ. of Mechanical Design, Vol. 101, No. 3, pp. 407417, 1979.
PAGE 180
BIOGRAPHICAL SKETCH Sujeet was born in Hyderabad, India on November 12, 1957. He graduated with a B.E. degree in electronics and communications engineering from the Osmania University, Hyderabad, in June 1979, with a Gold Medal for "The Best Allround Student." He joined the Department of Electrical Engineering, University of Florida in September 1979 and received an M.E. degree in March, 1981. Since then, he has continued work on a doctorate degree. Sujeet worked in the robotics division of the General Electric Co. as a summer intern during the summer of 1982 and 1983. He was hired by General Electric as a Ph.D. Intern in Sept. 1983. Although Sujeet' s primary interest is in pursuing research and teaching in the all captivating area of robotics, he intends to keep close ties with the industry. Sujeet is a member of the Institute of Electrical and Electronic Engineers, Phi Kappa Phi, and Tau Beta Pi. 170
PAGE 181
I certify that I have read conforms to acceptable standards adequate, in scope and quality. Doctor of Philosophy. this study and that in my opinion it of scholarly presentation and is fully as a dissertation for the degree of rei/th L. Doty, Chairman/ Professor of Electrical Engineering I certify that I have read conforms to acceptable standards adequate, in scope and quality. Doctor of Philosophy. this study and that in my opinion it of scholarly presentation and is fully as a dissertation for the degree of Giuseppe Basi4e Professor of Electrical Engineering I certify that I have read this study and that in my opinion it conforms to acceptable standards of scholarly presentation and is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. Thomas E. Bullock Professor of Electrical Engineering I certify that I have read conforms to acceptable standards adequate, in scope and quality. Doctor of Philosophy. this study and that in my opinion it of scholarly presentation and is fully as a dissertation for the degree of Douglas D. Dankel Assistant Professor of Computer and Information Sciences I certify that I have read this study and that in my opinion it conforms to acceptable standards of scholarly presentation and is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy.
PAGE 182
This dissertation was submitted to the Graduate Faculty of the College of Engineering and to the Graduate School, and was accepted as partial fulfillment of the requirements for the degree of Doctor of Phi losophy. December, 1984 Dean, College of Engineering Dean for Graduate Studies and Research
