UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

On-line smooth trajectory planning for manipulators Macfarlane, Sonja 2001

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata

Download

Media
831-ubc_2001-0459.pdf [ 9.19MB ]
Metadata
JSON: 831-1.0090209.json
JSON-LD: 831-1.0090209-ld.json
RDF/XML (Pretty): 831-1.0090209-rdf.xml
RDF/JSON: 831-1.0090209-rdf.json
Turtle: 831-1.0090209-turtle.txt
N-Triples: 831-1.0090209-rdf-ntriples.txt
Original Record: 831-1.0090209-source.json
Full Text
831-1.0090209-fulltext.txt
Citation
831-1.0090209.ris

Full Text

On-Line Smooth Trajectory Plannin for Manipulators by Sonja Macfarlane B.Sc.Eng., The University of New Brunswick, 1999  A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE FACULTY OF GRADUATE STUDIES (Department of Mechanical Engineering) We accept this thesis as conforming to the required standard  THE UNIVERSITY OF BRITISH COLUMBIA August 2001 © Sonja Macfarlane, 2001  In  presenting  degree at the  this  thesis  in  University of  partial  fulfilment  of  the  requirements  for  an advanced  British Columbia, I agree that the Library shall make it  freely available for reference and study. I further agree that permission for extensive copying of  this thesis for  department  or  by  his  or  scholarly purposes may be granted her  representatives.  It  publication of this thesis for financial gain shall not permission.  Department of  Mrvlw^l  E»y*e*ir>*  The University of British Columbia Vancouver, Canada  Date  DE-6 (2/88)  A^ait  ^ M Q A I  is  by the head of  understood  that  copying  my or  be allowed without my written  Abstract This work proposes a smooth, on-line, trajectory generation method for manipulators. The ability to compute a trajectory on-line increases the ease and speed with which manipulator programming can be done. It also reduces a manipulator's downtime and provides a certain amount of autonomy to the manipulator. These properties are especially desirable in industrial applications. Smooth trajectories are used to avoid controller and actuator saturation thereby reducing actuator wear, and improving tracking accuracy. They also improve manipulator motion times. Existing trajectory generation strategies can be divided into two groups: those based on the dynamic model of the manipulator, and those based on the kinematic limits of the actuators. Trajectory generation methods based on the dynamic model of the manipulator typically cannot be implemented on-line due to the required optimization processes, which can have widely varying computational requirements. Furthermore, their complexity is prohibitive for industrial implementation. Those based on the kinematic limits of the actuators provide computationally simpler solutions and can be used on-line. However existing kinematic-based trajectory planning algorithms that are suitable for use on-line, either do not incorporate jerk limits or involve the use of transcendental functions to limit jerk. The latter method increases the computational load. The trajectories proposed herein can be tracked using typical industrial controllers. The solution is based on forming a fifth order polynomial (quintic) trajectory following a sine-wave template, such that the velocity, acceleration and jerk limits are respected throughout the trajectory. All trajectories between two points can be planned by calculating the time, position, speed and acceleration at a maximum of seven control points, which describe the trajectory. This provides a tractable near time-optimal trajectory which can easily be generated on-line. The trajectory paths consist of straight lines joined by splines (blends) to ensure that the manipulator does not stop at each way-point. Limitations on the blend speed are proposed to ensure that the acceleration and jerk limits are respected during the blend. In addition, a high level planning architecture is provided for on-line trajectory planning. An implementation of the proposed trajectory generation technique and its comparison to an industrial technique show improved motion times, consistent kinematic profiles and improved path tracking.  ii  Table of Contents Abstract  ii  Table of Contents  iii  List of Tables  viii  List of Figures  x  Terminology  xii  Nomenclature  xiii  Acknowledgements  xviii  1 Introduction  1  1.1. Background  1  1.2. Industrial Motivation  2  1.3. Research Objectives  3  1.4. Thesis Outline  •••  3  2 Literature Review  5  2.1. Path Definition  5  2.2. Time-Optimal Trajectory Planning with Dynamic Limitations  6  2.3. Practical Implementation of PCTOM Trajectories  8  2.3.1. Controller Based Tracking of Purely Time-Optimal Motions  8  2.3.2. Incorporation of Torque Rate Limitations (Smooth Time-Optimal Motion)  9  2.4. Trajectory Generation with Kinematic Limitations  10  2.4.1. On-Line Trajectory Generation  10  2.4.2. On-Line Kinematic Trajectory Planning  12  2.4.2.1. Pick-and-Place Operations  12  2.4.2.2. Multiple Way-Points with Prescribed Path  13  2.4.2.3. Kinematic Limitations at, and Between, Way-Points  14  2.5. Summary of Trajectory Planning Techniques  16  2.6. Problem Formulation  16  2.6.1. Implementation  18  2.6.1.1. Task Space Planning Versus Joint Space Planning iii  18  Table of Contents  iv  2.6.1.2. Path-Related Constraints  19  2.6.1.3. High Level Planner  19  2.7. Summary  20  3 Methodology  21  3.1. Trajectory Functions (Building Blocks)  21  3.1.1. Sine wave template  23  3.1.2. Non-Limited Trajectory  25  3.1.3. Limited Trajectories  28  3.2. General Trajectory Generation Between Any Two Way-Points  30  3.3. Summary  35  4 Blends  36  4.1. Lloyd and Hayward's Blending Function  37  4.2. Geometric Blends  38  4.2.1. Conditions Necessary for a Geometric Blend  39  4.2.2. Velocity, Acceleration and Jerk Profdes for a 2D Geometric Blend  41  4.2.3. Limiting Acceleration and Jerk  43  4.2.4. Extension into n-dimensions  44  4.3. Summary 5 Connecting the Dots 5.1. Task Space Straight-Line Mode  45  4 6  47  5.1.1. Cartesian Dimensions  47  5.1.2. Orientation  49  5.1.3. Combining Cartesian and Orientation Trajectories and Generating a Trajectory  51  5.2. Joint Space Mode  52  5.2.1. Straight Line Joint Space Motion  52  5.2.2. Independent Joint Motion  53  5.3. Planning a Straight-Line Motion Between Two Way-Points With Variable Speeds  53  5.4. Backward Planning Issues and Trajectory Generation Between N Way-Points  56  5.5. Summary  58  Table of Contents  v  6 Application to a Manipulator  61  6.1. Way-Points  62  6.2. High Level Planner  62  6.2.1. Preliminary Considerations  62  6.2.2. Vetting  63  6.2.3. Trajectory Planning and Parser Points  66  6.2.3.1. Flag Setting Example  67  6.3. Parser  68  6.3.1. Straight Line Parsing  68  6.3.2. Task Space Parsing  69  6.3.3. Blend Parsing  70  6.4. Summary  70  7 Experimental Results  71  7.1. Experimental Setup  71  7.2. Comparison to a Typical Industrial Trajectory Generation Method  73  7.2.1. Task-Space Stop Point to Stop Point Motion  74  7.2.1.1. Experiment 1: Task-Space Straight Line with Constant Orientation  74  7.2.1.2. Experiment 2: Task-Space Straight Line with Constant Orientation  78  7.2.1.3. Experiment 3: Task Space Motion with a Change in Orientation  82  ^ 7.2.1.4. Summary of Results for Task Space Stop Point to Stop Point Motions  83  7.2.2. Task-Space Multiple Way-Point Motion  84  7.2.3. Joint-Space Stop Point to Stop Point Motion  86  7.2.3.1. Experiment 1: Joint Space Straight Line  86  7.2.3.2. Experiment 2: Joint Space Straight Line  89  7.2.3.3. Summary of Joint Space Stop Point to Stop Point Motion Results  89  7.2.4. Joint Space Multiple Way-Point Motion  92  7.3. Comparison to SPCTOM  94  7.4. Computation Time Issues  95  7.5. Summary  95  8 Conclusions & Recommendations 8.1. Conclusions 8.2. Recommendations  97 '  97 98  Table of Contents  yi  Bibliography  100  Appendices A Jerk Considerations  104  B Quintic End Conditions  107  B. 1. Quintic End Conditions for a Trajectory Based on a Sustained Acceleration Pulse  107  B.2. Quintic End Conditions for a Trajectory Based on an Acceleration Pulse  108  B.3. Algorithm followed when si > s2  109  C Algorithm Proof  Ill  D Quintic Control Points for Case B (iia)  114  E Blend Acceleration and Jerk Limits  115  E . l . Constant of Proportionality  116  E.2. Derivation of the Relationship Between the Maximum Acceleration and the Speed for a Geometric Blend  119  E. 3. Derivation of the Relationship Between the Maximum Jerk and the Speed for a Geometric Blend F Estimation of Task Space Kinematic Limits and Calculation of Joint Space Path Limits F. l . Joint Space Kinematic Limits F. 1.1. Joint Space Path Limits F. 2. Task Space Path Kinematic Limits G Quaternions  120 124 124 125 126 128  G. l . Rotation Matrices and Quaternions  128  G.2. Quaternion Multiplication and Transpose  129  G.3. Algorithm Used to Generate a Rotation Matrix at Each Sampling Interval  130  Table of Contents H Further Experimental Results H. 1. Stop Point to Stop Point Task Space Motions  vii 132 132  H. 1.1. Task Space Experiment 1  132  H.1.2. Task Space Experiment 2  133  H.1.3. Task Space Experiment 3  134  H. 1.4. Task Space Experiment 4  135  H. 1.5. Task Space Experiment 5  136  H.1.6. Task Space Experiment 6  137  H. 1.6.1 Explanation of Results for Task Space Experiment 6 H.2. Multiple Way-Point Task Space Motions  138 139  H.2.1. Horizontal Rectangular Motion  139  H.2.2. Vertical Rectangular Motion  140  H.3. Stop Point to Stop Point Joint Space Motions  142  H.3.1. Joint Space Experiment 1  142  H.3.2. Joint Space Experiment 2  143  H.3.3. Joint Space Experiment 3  144  H.3.4. Joint Space Experiment 4  145  H.4. Multiple Way-Point Joint Space Motion  146  List of Tables 6.1. Way-Point Speeds and Flags at the End of Planning  67  7.1. Denavit-Hartenberg Parameters for the Positional dof of the SCORBOT ER-VII  71  7.2. Denavit-Hartenberg Parameters for the CRS A465 Manipulator  72  7.3. Joint Kinematic Limits for the A465  73  7.4. Task Space Kinematic Limits for the A465  74  7.5. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 1  76  7.6. Motion Times and RMS Errors of Task Space Experiment 1  78  7.7. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 2  80  7.8. Motion Times and RMS Errors Corresponding to Task Space Experiment 2  81  7.9. Coordinates of Orientation Motion Start and End Points  83  7.10. Angle Between the Desired Rotation Axis and the Actual Rotation Axis at Time Increments for Task Space Experiment 3  83  7.11. Coordinates of Way-Points Corresponding to Figure 7.12  84  7.12. Motion Times for Task Space Multiple Way-Point Path  85  7.13. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 1  86  7.14. Motion Times and Accuracy with which the End Point is Reached for Joint Space Experiment 1 . 88 7.15. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 2  89  7.16. Motion Times for Straight Line for Joint Space Experiment 2  89  7.17. Coordinates of Way-Points for Multiple Way-Point Joint Space Path  92  7.18. Motion Times for Multiple Way-Point Joint Space Path  93  7.19. Motion Times for SPCTOM Trajectory  94  B. 1. Quintic Control Point Conditions for the SAP Algorithm  108  B.2. Quintic Control Point Conditions for the Acceleration Pulse Algorithm  109  H. 1. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 1  132  H.2. Results for Task Space Experiment 1  132  H.3. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 2  133  H.4. Results for Task Space Experiment 2  133  H.5. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 3  134  H.6. Results for Task Space Experiment 3  134  H.7. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 4  135  H.8. Results for Task Space Experiment 4  135 viii  List of Tables  ix  H.9. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 5  136  H.10. Results for Task Space Experiment 5  136  H. 11. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 6  137  H.12. Results for Task Space Experiment 6  137  H. 13. Coordinates of Way-Points for Task Space Horizontal Rectangular Path  139  H.14. Results for Horizontal Rectangular Path  139  H. 15. Coordinates of Way-Points for Task Space Vertical Rectangular Path  140  H.16. Results for Task Space Vertical Rectangular Path  141  H. 17. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 1  142  H.18. Results for Joint Space Experiment 1  142  H.19. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 2  143  H.20. Results for Joint Space Experiment 2  143  H.21. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 3  144  H.22. Results for Joint Space Experiment 3  144  H.23. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 4  145  H.24. Results for Joint Space Experiment 4  145  H.25. Coordinates of Way-Points for Multiple Way-Point Joint Space Path  146  H.26. Motion Time for the Multiple Way-Point Path at Varying Demanded Speeds  146  List of Figures T. 1. Trajectory Segment Terminology  xii  2.1. Proposed Acceleration Profiles  14  3.1. Linear Segment with Parabolic Blend (LSPB) Trajectory Between Way-Points p 1 and p2  22  3.2. Sine Wave Template for a Positive Change in Acceleration  24  3.3. Smooth Trajectory Generated with a Sequence of Quintics  26  3.4.  29  Speed Limited Trajectory  3.5. Flowchart Indicating the Algorithm to Use Based on the Way-Point Speeds  31  3.6. Illustration of Dlimit  33  3.7.  34  Illustration of Shortened Acceleration Segments  3.8. Template Used to Determine in Subcase iib  34  4.1. Blend in Time Domain  37  4.2. Effect of tightness on Path  ,  39  4.3. 2D Blend Geometry  40  4.4. Velocity Profdes for a 2D Geometric Blend  42  4.5. Acceleration Profdes for a 2D Geometric Blend  42  4.6. Jerk Profdes for a 2D Geometric Blend  43  5.1. Four Different Cases of Straight Line and Blend Motions  48  5.2.  Sketch of a Multiple Point Path  '.  49  5.3. Rotational Motion Trajectory  50  5.4.  Generation of Trajectories Between Any Two Way-Points pi and pi+1  54  5.5.  Cartesian Space Trajectory Reference Data Generation  55  5.6. Joint Space Trajectory Reference Data Generation  56  5.7. Three Possible Speed Profdes Between Three Way-Points  57  5.8. Trajectory Planning Between N Way-Points (Forward Planning)  59  5.9. Backplanning  60  6.1. Interaction Between the Three Planning Units  61  6.2. Vetting Procedure  64  6.3. Elimination of Way-Points Due to Tightness Considerations  65  6.4.  65  Special Cases for Point Elimination in Vetting Algorithm  7.1. Experimental Setup  72  7.2. Comparison of Tracking Performance of Task Space Experiment 1 x  75  List of Figures  x[  7.3.  Comparison of Speed During Motion of Task Space Experiment 1  76  7.4.  Comparison of Path Deviations of Task Space Experiment 1  77  7.5. Acceleration Profiles (filtered) of Task Space Experiment 1  77  7.6.  Comparison of Speed During Motion of Task Space Experiment 2  79  7.7.  Comparison of Tracking Performance of Task Space Experiment 2  79  7.8.  Comparison of Path Deviation During Motion of Task Space Experiment 2  80  7.9.  Comparison of Acceleration Profiles (filtered) of Task Space Experiment 2  80  7.10. Comparison of Orientation With Respect to World Frame During the Motion for Task Space Experiment 2  81  7.11. Comparison of at Different Maximum Allowable Speeds  82  7.12. Comparison of Paths at Different Speeds for Task Space Multiple Way-Point Path  85  7.13. Comparison of Speed Profiles for Task Space Multiple Way-Point Path  86  7.14. Comparison of Joint Velocities for Joint Space Experiment 1  87  7.15. Comparison of Industrial Planner and Quintic Planner Acceleration Profiles (filtered) for Joint Space Experiment 1 Trajectory with Demanded Speed of 1016 mm/s (100% MMAS)  88  7.16. Comparison of Joint Velocities for Joint Space Experiment 2  90  7.17. Comparison of Filtered Joint Acceleration Profiles for Different Jerk Limits  91  7.18. Comparison of Joint Velocity Profiles With Different Jerk Limits  92  7.19. Comparison of Joint Space Paths for Joint Space Multiple Way-Point Experiment  93  7.20. Comparison of Quintic Concatenation and SPCTOM Joint Performance  95  7.21. Comparison of Tracking for SPCTOM and Quintic Concatenation Method  95  A. 1. Jerk, Acceleration, Velocity and Position Profiles Corresponding to a Sine-Based Acceleration Ramp from 0 to amax  104  B. 1. Smooth Trajectory Generated with a Sequence of Quintics  108  B.2. Speed Limited Trajectory (• indicate the quintic control points)  109  E. 1. 2D Blend Geometry  115  E.2. Plot of Acceleration Profile for the 2D Blend of Figure E. 1  117  E.3. Figure Illustrating the Variables Used in Equations E.31 and E.32  119  E.4. Jerk Profiles for the Blend of Figure E.l  121  E. 5. Diagram Indicating the Geometry Used to Derive Eq. E.51  123  F. 1. Kinematic Limit Projection for a 3D Straight Line Path  126  H. 1. Comparison of Paths at Different Speeds for Vertical Rectangular Path  140  H.2. Comparison of Speed Profiles for Task Space Vertical Rectangular Path  140  Terminology Herein, various terms are used to describe the trajectory segments. These terms are modified by the parameter that they describe, and are depicted in Figure T. 1: i control points 1.5  pulse-  sustained pulse  ramp  ramp  0.5  0  0.5  1.5  2  2.5 time  3  Figure T. 1. Trajectory Segment Terminology ramp:  A change in speed, acceleration or jerk. For example, a speed ramp is a trajectory segment with a change in speed.  cruise:  Indicates no change in the relevant parameter, e.g. a speed cruise is a constant speed trajectory segment.  pulse:  Composed of a ramp-up followed by a ramp-down.  sustained pulse:  Composed of a ramp-up, a cruise, followed by a ramp-down.  Three types of points are frequently referred to in this document. They are: way-points:  Points, provided by the user, designating positions that the machine must pass through. A way-point may be defined in either joint space or task space.  control points:  Points that provide the end conditions for the quintic polynomials in a trajectory. Control points define a trajectory between two way-points. A control point is assigned time, position, velocity and acceleration.  stop points:  Way-points with a zero speed condition.  Two spaces are frequently used in this document to define the position of a manipulator: task space:  The position vector is defined as a combination of the Cartesian coordinates (i.e. the positional coordinates, x, y, and z) and the orientation coordinates (y/, y/ , i/ ). x  joint space:  The position vector is defined as a vector of joint positions. xii  y  z  Nomenclature Best  flag used in the high level of the trajectory planner  C  Christoffel symbols of the manipulator (centripetal and Coriolis coefficients)  D  Euclidian distance between two points  D|  distance covered during a ramp from zero acceleration to a  D|  max  distance covered during a ramp from zero acceleration to a  Z  in the SAP algorithm  max  in the SAP algorithm for  the case z D  distance covered during the acceleration cruise at a  max  in the SAP algorithm  D2z  distance covered during the acceleration cruise at a  max  in the SAP algorithm for the case z  D3  distance covered during a ramp down from a  max  distance covered during a ramp down from a  max  2  D3  Z  to zero acceleration in the SAP algorithm to zero acceleration in the SAP algorithm  for the case z D  a  distance covered during a SAP used to ramp from si to si , (=D, + D + AO  D  b  distance covered during an acceleration pulse from s to su ,  imi  2  a  2a  mi  minimum distance required to be able to use the SAP when ramping from si to s  Diimit'  peak  and  then from s k to s . pea  2  D in  minimum distance required to use the SAP algorithm  D  distance required to ramp between the two way-point speeds  m  v  D , D,D,D w  x  y  z  distances required to ramp between two speeds  Desirable  flag used in the high level of the trajectory planner  F  world frame  w  G  vector of gravity terms  H  unit quaternion  J(y)  Jacobian matrix at joint configuration y  K  constant used to normalize a quaternion  L  variable used in determining the root of an equation  M  inertia matrix of the manipulator  N  number of way-points between which a trajectory should be planned at one time (size of planning window)  N onst  constant used to normalize a quaternion  P  variable used in determining the root of an equation  C  xiii  Nomenclature R  xiv rotation matrix describing the rotation necessary to move from the orientation at point x  xy  to the orientation at point y S(f)  skew symmetric matrix corresponding to vector/  T  vector of actuator torques  U, V  variables used in determining the root of an equation  W  number of points preceding the last Best point in the window of N way-points  Y  number of points given to the parser from the window of N way-points  Z  quaternion = [Z , Z , Z , Z ]  a(t)  acceleration at time t  a  acceleration vector of Cartesian degrees of freedom  a a  w  x  y  z  ) a  vector of accelerations at the start of a blend  2a  vector of accelerations at the end of a blend  awendMax  maximum acceleration occurring during a blend.  a  manufacturer maximum allowable acceleration (MMAA)  max  apeak  peak acceleration reached during a point to point motion (< a )  bi, in,  intercept and slope of xi(ti)  max  b, m 2  b  2  intercept and slope of x (d) 2  polynomial coefficients (e.g. bo, b|, b )  x  2  dD  distance remaining after an acceleration pulse (used in the speed-limited case)  ds  change in speed  ds in  minimum change in speed required to use the SAP algorithm  dSrd  change in speed required for a ramp from maximum acceleration to zero acceleration in  m  the SAP algorithm dt dt  time required to ramp up to a k while respecting j pea  time required for a ramp up to a  max  max  f  (< dt ) max  from zero acceleration while respecting j  max  axis of the axis-angle representation of a rotation matrix = \f , f , f.] x  g(t) n  max  s\  quintic polynomial *  i, j, k  unit vectors forming an orthogonal frame  j(t)  jerk at time t  jbiendMax  maximum jerk occurring during a blend  j  maximum allowable jerk  m a x  k, 1, m  coefficients of a cubic equation  y  Nomenclature  xv  n  number of actuators or joints  p(t)  position at time /  p  coordinates of the point at the end of a straight line segment  Pi  position at point i  Pi  position vector at point / (in joint or task space)  e  p  ia  vector of positions at the start of a blend around point /?,  p  ib  vector of positions at the end of a blend around point /?,  p  s  coordinates of the point at the start of a straight line segment  q(t)  trajectory polynomial  q(t), q(t), 'q(t) trajectory derivatives r 11,..., r  33  elements of the rotation matrix  s(t)  speed at time /  s%  speed expressed as a percentage of the Manufacturer Maximum Allowable Speed  s  speed attained at the end of a ramp from zero acceleration to maximum acceleration in  a  the SAP algorithm Saz  speed attained at the end of a ramp from zero acceleration to maximum acceleration in the SAP algorithm for the case z  Sb  speed attained at the end of the acceleration cruise in the SAP algorithm  Sb  speed attained at the end of the acceleration cruise in the SAP algorithm for the case z  SbiendMax  maximum speed with which a geometric blend can be started or ended while respecting  Z  acceleration and jerk limits. SwendMaxAcc  maximum speed with which a geometric blend can be started or ended while respecting the acceleration limit.  SbiendMaxJerk  maximum speed with which a geometric blend can be started or ended while respecting the jerk limit.  Si  speed at point i  siimit  speed reached by an acceleration pulse at a  Smax  maximum allowable speed along a trajectory segment (< MMAS)  s^k  peak speed reached during a trajectory between two way-points (< s )  t  time  t  cart  max  max  time required to carry out the desired change in position (Cartesian space) while respecting kinematic limits  t  K  time at sampling interval k  Nomenclature t  xvi time required to carry out the desired change in orientation while respecting kinematic  rot  limits t  time at which x,(t) and x {f) intersect  s  2  t|  time required to carry out a given straight line motion  tx  rotation time limit based on respecting the rotational acceleration limit  t<p  rotation time limit based on respecting the rotational jerk limit  t  rotation time limit based on respecting the rotational speed limit  S  ro  tightness  variable that indicates the size of the blend  u  unit vector  v  velocity vector of Cartesian degrees of freedom  Vi  vector of velocities at the start of a blend  a  v  2a  vector of velocities at the end of a blend  v  d  =  m - m, 2  x[(t), x (t)  position trajectory polynomials that are connected by a blend  X|(o), x (c)  x,{t) and x (i) expressed in the cr-domain  At  controller sampling interval  Atjerk  time allowed to ramp up to maximum acceleration (used in calculating the jerk limits of  2  2  2  the actuators CC(G)  blend function that smoothly increases from 0 to 1 over the interval o~ G [0,1]  (3(0")  blend function used to compensate for extra acceleration  y  vector of joint angles  y  vector of joint velocities  Y  vector of joint accelerations  Y  m a x  Y  max  vector of joint space acceleration limits vector of joint space jerk limits  6  angle between the lines described by Xi(d) and x (d)  K  acceleration compensation factor used in blending  2  W  maximum allowable rotational acceleration (manufacturer limit)  p  blend acceleration proportionality constant  mm  Nomenclature  xvii  a  normalized time coordinate used in blending, ere [0,1]  Ok  cat sampling interval k  x  half of the total blend time time required to complete half blend u  <> l  angle of the axis-angle representation of a rotation matrix  X  snap maximum allowable rotational jerk (manufacturer limit)  (Pmax  roll, pitch, yaw angles maximum allowable rotational velocity (< manufacturer limit)  tOmax  dof  degrees of freedom  DH  Denavit-Hartenberg  LSPB  Linear Segment with Parabolic Blends  MMAA  Manufacturer Maximum Allowable Acceleration  MMAS  Manufacturer Maximum Allowable Speed  ORTS  Open architecture Real-Time operating System  PCTOM  Path-Constrained Time-Optimal Motion  PID  Proportional Integral Derivative  SAP  Sustained Acceleration Pulse  SPCTOM  Smooth Path-Constrained Time-Optimal Motion  Acknowledgements I would like to thank my supervisor, Dr. Elizabeth Croft, for her guidance and support throughout my two years here, and for always being there to answer my numerous questions. I would also like to thank all the members of the IAL for their support and friendship, and for humouring me when I felt a little stressedout. The assistance of the faculty and staff of the Mechanical Engineering Department was greatly appreciated. A special thanks to my parents for always encouraging me in my chosen fields of study, and for nodding and smiling at the right times when I was explaining some deep, but perhaps useless, fact to them. And finally, thanks to my brothers, for being my brothers. I would also like to acknowledge the financial support of the Natural Sciences and Engineering Research Council of Canada.  xviii  Chapter 1 Introduction 1.1. Background Multi-axis machines are used in a variety of applications: pick-and-place operations, welding, machining, etc. Such machines can be divided into two units: the physical mechanism composed of links and actuators, and the control system. The number of actuators present in the mechanical system depends on the number of independent machine axes, or degrees of freedom. For example, a typical articulated six degree of freedom manipulator contains six rotating actuators. A five-axis high-speed CNC machining centre would contain three linear actuators and two rotating actuators. A motion task given to the machine must ultimately be represented as a reference signal, which is sent to the control system. The control system acts to make the machine track the reference signal by activating the appropriate actuators. If the reference signal changes too quickly, given the dynamic limitations of the machine, the tracking of the reference signal will be poor, regardless of the control system design. Computer algorithms are designed to calculate an appropriate reference signal based on the desired task path and time-related limits (such as speed and acceleration). This reference signal is the trajectory, and can be defined as a locus of points in operational or joint space on which a time-law has been specified [42]. The generation of an appropriate trajectory is the problem that is being investigated in this thesis. The path along which the trajectory is defined can be point-to-point; namely, the machine is required to move between the two points but is not given any fixed intermediate path. This type of path is useful in manipulator pick-and-place operations. A path can also be completely specified through use of geometric functions. This type of path is commonly used in CNC machining applications or in manipulator applications when obstacles are present, or when it is necessary to ensure that the end effector follows a specific path. Herein it is assumed that the path definition is provided, and the problem of path planning is not specifically addressed. The control of the machine motion can be divided into two parts: motion planning and motion tracking. Motion planning involves generating the path and its time law, providing the controller's reference signal. Motion tracking, on the other hand, is concerned with improving the tracking of the reference signal. 1  Chapter 1  2  Motion planning is often done off-line, typically when the trajectory generation algorithms are computationally intensive. However, it is often desirable to generate trajectories on-line so that changes can easily be made to the machine's trajectory, increasing the system's overall robustness and adaptability. For example, a manipulator may require the ability to recompute its trajectory on-line in order to avoid an unexpected obstacle that lies along a path on which it is currently moving [3]. In an automated robot workcell, a high level scheduler feeds a series of tasks to a manipulator in terms of waypoints, approach points and stop points. The manipulator must execute the task by generating paths and trajectories to these points and then following these trajectories using a control law. Use of an on-line planner reduces manipulator setup time and downtime, since the time required to plan the trajectories is shorter. In industrial applications, it is common to use simple PID control laws, which do not take into account the system's nonlinearities, to track a reference signal. To compensate for tracking errors introduced by the system's nonlinearities, a more complicated controller must be used [17, 18, 27]. Alternatively, it is possible to design a trajectory that takes the system's nonlinearities into account and thus provides a reference signal that can be more easily tracked by common industrial controllers [4, 14]. 1.2. Industrial Motivation Increased productivity is an important industrial consideration. When a multi-axis machine limits the task speed, decreasing the machine's overall motion time will increase productivity [4, 17]. In addition, improving the tracking accuracy of the machine is always desirable since it results in more repeatable products or operations. Tracking a purely time-optimal trajectory with a simple controller will saturate the actuators resulting in poor tracking, vibrations in the machine and increased machine wear [5, 30, 32, 40, 48, 52]. Specialized controllers have been developed in order to provide better tracking of time-optimal trajectories. However, it is unlikely that they will be widely implemented in industry due to their complicated form. Purely timeoptimal trajectories have been modified to take into account further limitations of the actuators, for example jerk or torque rate limits, thereby avoiding controller saturation and resulting in improved tracking accuracy [7, 14, 45]. Herein, trajectories that are planned with jerk or torque rate limitations are termed smooth trajectories.  There exists a need for a smooth trajectory generation algorithm that can easily be integrated into existing industrial systems, that is, be implemented using a typical industrial controller. Such an algorithm should  Chapter 1  3  be applicable on-line, provide adequate dynamic limitations, and allow the specification of the speeds at all the way-points. 1.3. Research Objectives The objective of this work is to provide a method of generating smooth, near time-optimal trajectories, on-line. Therefore, the trajectory generation algorithm must be computationally simple. The trajectories must take into account the physical limitations of the machine, that is, not only their torque limitations but also their torque rate limitations. This work aims to produce a trajectory that will improve the motion time and the trajectory tracking of a standard industrial machine whether it is applied to the machine through a simple PID controller or a more complex controller. The trajectory is to be planned such that it could be used to provide a time-law for any type of path, described in any space, provided that the corresponding path jerk, acceleration and velocity limits are derived. A further objective is to apply this technique to a robotic manipulator. To ensure the practicality of the proposed approach, guidelines were provided by a robotics manufacturer [19]. The guidelines specified that the motion generator should include: • the ability to provide straight line, jerk-limited motion between way-points with variable speeds; • a cornering scheme for motion around trajectory way-points, such that the motion does not stop at each way-point; • coordinated joint motion (i.e., all the joints must start and stop at the same time); • an overall planning algorithm that should allow trajectory planning for a window of N way-points "on the fly" considering the desired position and velocity at each way-point. Furthermore, the trajectory generation algorithm should be applicable in both joint and task space. That is, the trajectory way-points can be defined in either task space coordinates, with straight-line motion between the way-points, or the way-points can be defined in joint space, with the motion between the way-points being planned in joint space. Finally, it is desirable that no retrograde motion occur during the trajectory.  1.4. Thesis Outline This chapter introduces the notion of motion planning for multi-axis machines. The research objectives were presented, and an outline of the thesis is provided below. The terminology specific to this thesis and frequently used herein is defined in the Terminology section (p. xii).  Chapter 1  4  Chapter 2 discusses the work that has been done in the area of motion control, and justifies the approach used in this thesis. Chapter 3 presents the method used to generate a trajectory between two points in a one-dimensional parametrized space. The basic trajectory planning algorithm described in Chapter 3 can be adapted for use on different types of machines such as manipulators, CNC machines, cranes, etc. where a reference trajectory is used to describe the machine's task. This work then concentrates on the implementation of the trajectory generation algorithm of Chapter 3 on a robotic manipulator, based on the guidelines for robotic motion described in Section 1.3. In Chapter 4, the issues related to blending motions between straight line segments without stopping are addressed and a solution proposed. In Chapter 5, the method derived in Chapter 3, combined with blending is extended into n-dimensions. In addition, the overall trajectory generation algorithm designed for a manipulator system is explained in Chapter 6. Chapter 7 presents experimental results comparing an implementation of the trajectory generation algorithm developed herein to an industrial planner, using a CRS A465 manipulator. Examples of motions in both joint and task space are presented. In addition, the method is compared to a time-optimal, with torque rate limitations, motion using a SCORBOT ER-VII manipulator. Finally, Chapter 8 presents the conclusions of this research, as well as recommendations for future work in this area.  Chapter 2 Literature Review This chapter discusses issues relevant to trajectory generation for manipulators. Existing trajectory generation methods that produce time-optimal motion and near time-optimal motion are discussed. Both off-line and on-line trajectory generation techniques are examined since the time-optimal, off-line trajectory generation algorithms provide a basis against which sub-optimal on-line trajectory generation algorithms can be compared. Initially minimum time trajectories are explored since they address the need to increase productivity. However, these trajectories do not take into account the fundamental torque rate limitations of the actuators and result in controller saturation and poor tracking accuracy. Methods of improving the tracking of such trajectories are then discussed and trajectories planned considering the torque rate limitations, smooth trajectories, are expounded in Section 2.3. These methods all require the off-line generation of trajectories due to their high computational load. On-line trajectory generation is desirable since it reduces manipulator setup time and downtime. These methods consider only the kinematic limitations of the manipulator and result in near-time optimal motion. The kinematic limits that must be considered to provide smooth motion are speed limits, acceleration limits (proportional to torque) and jerk limits (proportional to torque rate). These methods are discussed in Section 2.4 and emphasis is placed on the methods that limit the kinematics during the entire trajectory, not just at the way-points. In Section 2.6 the research objectives presented in Chapter 1 are expanded and justified. 2.1. Path Definition A manipulator task may be defined as a series of way-points, through which the manipulator should pass (unconstrained motion). However, it is often desirable to specify the path between the way-points to avoid obstacles present in the workspace, for example, or to facilitate a task such as spray-painting or welding. In robot manipulations, such tasks are generally described by straight-line motions in task space joined by polynomial splines at the way-points. The splines pass through or near the way-points so that the manipulator does not have to stop at each way-point [33, 35, 50]. The objective in this constrained motion 5  Chapter 2  6  is to accurately track the path [43]. Trajectory planning methods for both types of path specification have been developed and will be discussed below. In this thesis, it is assumed that the path to be followed by the manipulator can be parametrized in terms of a single variable. The path is given in terms of the actuator positions (joint-space specification) or in terms of the placement of the tool tip in the workspace (task-space specification). 2.2. Time-Optimal Trajectory Planning with Dynamic Limitations One of the most important issues in trajectory planning for manipulators is that of increasing productivity. Machine processing or handling time can be a system bottleneck. Such machines represent high capital costs, therefore increasing the number of machines is often not economically feasible. The capability of the machine actuators limits the motion time. However, increasing the size and power of the actuators is not always desirable since this would increase the inertia of the overall system. Such a solution can be largely self-defeating [4]. Another approach is to use minimum-time trajectories. Time-optimal control of manipulators is a difficult problem due to the manipulators' nonlinear, coupled dynamics and their joint and torque limits [28], The first attempts at solving this problem from a trajectory planning viewpoint resulted in suboptimal control solutions. Kahn and Roth (1971) formulated the time-optimal control problem for an unconstrained path subject to torque limitations using Pontryagin's principle [26]. The resulting set of equations was computationally intractable. Thus, the dynamic model of the manipulator was linearized so that a standard linear optimal control technique could be applied to the problem, and a near time-optimal trajectory obtained. However, linearization of the dynamics results in significant errors [39]. Geering et al. (1986) developed the structure of a time-optimal motion for unconstrained paths subject to torque constraints using Pontryagin's minimum principle, and showed that such motions were either bang-bang or bang-singular-bang in torque [24]. Chen and Desrochers proved that such a minimum-time control law requires that at least one of the control torques be saturated along the entire trajectory [13]. Sahar and Hollerbach developed a general solution for determining the time-optimal path of a point-topoint motion using joint-space tessellation to create a state-space search tree, which is searched to find the best solution [41]. The algorithm takes into account the full dynamic model of the manipulator and kinematic, dynamic and geometric constraints. Rajan, on the other hand, combined the use of Pontryagin's principle and dynamic search techniques to obtain a minimum-time point-to-point trajectory [39]. A path is first determined using a joint space search technique and then the algorithm in [4] is used  Chapter 2  7  to determine the minimum-time law along the path. The path is then varied and the minimum time-law along the new path found. This is done iteratively until the minimum time path and trajectory are obtained. Bobrow et al. [4] and Shin and McKay [44] considered the problem of moving a manipulator along a specified path in minimum-time while taking the manipulator torque limits into considerations. Shin and McKay parametrized the path in joint space while Bobrow parametrized it in task space. Both use phase plane techniques to determine the trajectory switching points, which indicate where a change from maximum acceleration to maximum deceleration or vice-versa should occur, in order to obtain minimumtime motion along the path. The resulting path-constrained time-optimal motion (PCTOM) maximized the velocity along the given path. The acceleration and velocity limits are calculated directly from the robot's dynamic equations and the given path. Pfeiffer and Johanni provide an efficient manner of identifying the switching points [36]. Both Shin and McKay and Bobrow et al. showed that the optimal control along a parametrized path was bang-singular-bang. Furthermore, Chen and Desrochers showed that the PCTOM solution requires that only one control torque always be in saturation on every finite interval along the time-optimal trajectory [12].  Kim and Shin developed a minimum-time path planning method in joint space based on the dynamic model of the manipulator and subject to torque constraints [28]. The path was formed of a series of straight lines with specified path deviation at the corner points. By deriving bounds on the joint space acceleration from the manipulator torque limits based on a heuristic approximation, the problem was divided into a set of one-dimensional optimization problems which could easily be solved. Their approach differed from that of Bobrow et al. and Shin and McKay in that it allowed for path deviations at corner points. However, as discussed, time-optimal trajectories based on the dynamic model of the machine and the actuator torque limitations result in discontinuous (bang-bang) actuator torque reference profiles. Such rapid torque fluctuations cannot be obtained from the actuators because of their physical limitations [5, 14, 30, 43], and as a result, the actuators fall behind the reference signal, leading to controller saturation, which produces vibrations in the machine and tracking errors. Furthermore, the computational burden of generating such trajectories for manipulators is very high. The methods described above require, in essence, the solution of a two point boundary value problem for n degrees of freedom for a non-linear system. This is done through dynamic programming and/or other optimization/search techniques. Work  Chapter 2  8  by Croft et al. [16] and Sahar and Hollerbach [41] considered strategies for reducing the computational expenses of solving this problem through heuristics and approximation techniques. 2.3. Practical Implementation of P C T O M Trajectories PCTOM trajectories result in at least one saturated actuator at all times. In addition, the torques may be discontinuous at the switching points. Such trajectories cannot be accurately tracked since they do not take into account the uncertainties of the dynamic model, the dynamics of the tracking controller and requirements for tracking accuracy [27]. Furthermore, the actuator saturation leaves no control authority to correct the tracking errors that will occur [17, 18, 27, 43]. Tracking accuracy is of prime importance in many applications such as welding, painting, gluing, etc. Therefore, PCTOM-type trajectories are of little use in practice unless these effects can be overcome. This can be done by introducing changes at the controller level [17, 18, 27, 43, 46] or by modifying the PCTOM trajectory [14, 45]. The first approach is discussed in section 2.3.1, while the second is discussed in Section 2.3.2.  2.3.1. Controller Based Tracking of Purely Time-Optimal Motions Tarn and Potts proposed a perturbation control scheme for tracking PCTOM trajectories [46]. In this scheme, the control signal, switching time and final time are updated based on the path deviation. The method was compared to computed torque control and was found to provide faster convergence to the path and less overshoot since it accounts for actuator saturation. However, this method does not eliminate actuator saturation: it is simply postponed. If an actuator is already at its torque limit, it is obviously not possible to catch up to the nominal trajectory. One way of improving path tracking is to design the trajectories with conservative torque limits; another is to modify the trajectory on-line [17]. Dahl and Nielsen argue that the second method is preferable, since being conservative in the trajectory planning stage is inefficient for productivity reasons [17]. That is, it is hard to ensure that the conservative torque limits chosen are not too conservative. They propose a scheme in which the primary robot controller is left unchanged, while the time-optimal reference trajectory is modified using a secondary control loop, the path velocity controller. The path velocity controller modifies the nominal trajectory to achieve a trajectory that does not require more torque than is available. The path velocity controller limits the path acceleration by modifying the nominal velocity profile at the expense of increased motion time. This method provides good tracking accuracy and good utilization of the available torque range [18]. This method allows and adjusts reference trajectories that exceed the robot tracking capabilities and increases robot performance since it is able to approach the minimum possible motion times.  Chapter 2  9  Shiller et al. [43] proposed an implementation which resulted in higher tracking accuracy at high speeds than that provided by Dahl and Nielsen's work [17, 18]. It is based on using trajectory preshaping to compensate for the delay introduced by the feedback controller used to track a PCTOM trajectory, as well as a simplified motor model that models system dynamics not taken into account in trajectory generation, and is used to modify the preshaping current command. Comparison with a typical linear segments with parabolic blends (LSPB) profile showed a six-fold reduction in tracking error. Kieffer et al. noted that all the above schemes did not allow a minimum path deviation tolerance to be specified [27]. They proposed two schemes to adapt time-optimal trajectory planning algorithms for application to manipulators under computed torque control while tracking the paths to a prescribed tolerance. In the first scheme, the controller gains are chosen experimentally while tracking a PCTOM trajectory. The required compensation torques are then calculated to meet a prescribed tracking accuracy. A PCTOM with those new torque limits is calculated and used along with the experimentally determined controller gains. This scheme resulted in the elimination of actuator saturation, tracking accuracy within the desired tolerance, and an increase in motion time of between 6 and 23%. In the second scheme, the reference trajectory is generated on-line as a feedback function of the plant and the controller states. However, offline planning of the path acceleration policy is necessary. This scheme resulted in improved motion time while maintaining equivalent tracking accuracy compared to the first. In 1987, Leahy and Saridis suggested a number of improvements to computed-torque compensation techniques aimed at improving tracking, namely friction compensation and improved frequency response [30]. They concluded that although these methods improved tracking accuracy, the controllers still could not overcome the jerk constraints that affected tracking accuracy, and as such, a trajectory generation algorithm that incorporated the jerk limits of the system was required. Bobrow et al. recognized that the discontinuous actuator torque required by the minimum-time solution cannot be exactly provided, given the dynamic properties of the actuators [4]. They concluded that the time constants relating the input voltage to the output torques were small enough to be neglected, but others have shown this assumption to be incorrect [5, 14, 29, 45, 48]. Methods that take into account the torque rates of the actuators giving the corresponding time-optimal motion are discussed in the next section. 2.3.2. Incorporation of Torque Rate Limitations (Smooth Time-Optimal Motion) As discussed above, while PCTOM motion yields a solution for time-optimal motion based on bang-bang control theory [48], actuator torques cannot be varied instantaneously (i.e. the torque rate must be limited). The drive system cannot produce the large value of current necessary when a large change in  torque is demanded [5]. Discontinuities in the actuator trajectory cause vibrations in the machine structure leading to tracking inaccuracies and wear on the machine [5, 7, 15, 30, 31, 52]. It is often more cost effective to use trajectories that take the torque rate limitations into account than to redesign the machine [52]. The use of torque constraints ensures efficient use of joint actuators, while torque rate constraints resolve the problem of bang-bang behaviour of joint torques [48]. In 1986, Shin and McKay presented a solution to the PCTOM problem incorporating torque rate constraints [45]. This solution involved the use of a dynamic programming method, which forced the authors to make assumptions on the variation of certain quantities within discretized regions [48]. Tan and Potts advocated the use of a complete discretization of the minimum-time trajectory problem rather than using discretization in the computation of the time-optimal solution based on a continuous manipulator model [48]. They argued that robot manipulators are generally controlled at discrete sampling intervals and that discrete computation methods are necessary due to the highly non-linear dynamics of the manipulator. The solution incorporated joint torque constraints, joint velocity constraints and torque rate constraints and was based on a discrete model of the manipulator dynamics. The planning of a trajectory along a path with the above constraints and 10 discrete time intervals required 2 s of CPU computation time, and a 1 min computation time was required for a case with 40 discrete time intervals. Tarkiainen and Shiller developed a method for computing a time-optimal trajectory that was bang-bang in jerk along a specified path, except at singular points and arcs where the acceleration was at its maximum or minimum [49]. Constantinescu and Croft also presented a method for determining smooth and timeoptimal  motions for path-constrained trajectories (SPCTOM) [15]. The controlled input of the  optimization problem is the pseudo-jerk, and cubic splines are used to parametrize the state space trajectory. It was proved experimentally that SPCTOM trajectories with appropriate torque rate limits provide a faster motion than a PCTOM or a motion described with a single quintic subject to equivalent kinematic limits. 2.4. Trajectory Generation with Kinematic Limitations 2.4.1. On-Line Trajectory Generation Manipulator dynamics are nonlinear and coupled. Thus, the time-optimal trajectory solution (with or without torque rate constraints) is, in general, too time-consuming for on-line implementation. Instead, the trajectory planning is done off-line, providing the necessary sequence of joint angles (and their subsequent derivatives depending on the machine control scheme in use). Path tracking is then carried out  Chapter 2  11  on-line [48]. An alternative to considering the full dynamic model of the manipulator is to derive the kinematic limits of the manipulator (i.e. velocity, acceleration and jerk limits) and to plan a trajectory using simple functions that stay within these limits. It is usually assumed that there are piecewise constant bounds on machine velocity and acceleration. In reality, these bounds vary with position, angular velocity and payload mass and shape because of the manipulator dynamics are nonlinear functions [4, 44]. Therefore, the constant bounds must be chosen as the reasonable global lower bounds of the velocity and acceleration in the entire workspace. This results in underutilization of the machine capabilities since the task space limits are determined experimentally and are for a 'worst case scenario' [28]. However, kinematic methods are well suited for on-line applications since the algorithms tend to have short calculation times and analytical solutions. Given the limits, kinematic time-optimal solutions can be developed but will never reach the full dynamic optimality. In either case, unless jerk is limited, the system (actuators and controller) is unlikely to be able to track the trajectory, making optimality a moot point. The extra computational effort and the complex mathematical formulations that are associated with offline methods are often not justified. A pre-planned trajectory is not exactly followed by the manipulator since there are errors in the mathematical model of the machines, due to their complexity [48]. In addition, the ability to compute a trajectory on-line increases the ease and speed with which manipulator programming can be done. It also reduces a manipulator's downtime and provides a certain amount of autonomy to the manipulator. A manipulator may require the ability to recompute its trajectory on-line in order to avoid an unexpected obstacle lying along a path on which it is currently moving [3]. For these more practical reasons which relate to industrial applications, as well as autonomous robotics applications, this research aims to develop an on-line trajectory generation algorithm. Trajectory generation techniques based on limiting the manipulator kinematic variables that involve optimization have been developed. For example, Cao et al. suggested methods of obtaining smooth minimum-time motion along both constrained and unconstrained paths [7, 8]. The algorithm involves initially determining smooth and time-suboptimal cubic spline joint paths through minimization of an objective function. This is followed by scaling the resulting travelling time so that velocities, accelerations and jerks at the knot points of each joint trajectory are maximized within their limitations. Due to the computational load of the optimization process, the trajectory must again be planned off-line. Since the research objectives of this thesis involve the on-line generation of trajectories, only the trajectory generation methods involving kinematic limitations that do not require optimization will be discussed further.  Chapter 2  12  2.4.2. On-Line Kinematic Trajectory Planning 2.4.2.1. Pick-and-Place Operations  Trajectory planning techniques have been developed for pick-and-place operations in which the manipulator is required to move an object from one position to another and the intermediate path is not defined [2, 25, 34]. In such techniques two way-points are always provided: the initial or pick configuration and the final or place configuration, generally specified in task space coordinates and having zero velocity conditions. The way-point coordinates are usually converted to joint space coordinates to allow the trajectory to be planned in joint space in order to reduce the computational complexity of the algorithms. Angeles et al. used C continuous periodic cubic spline segments in joint 2  space to form a trajectory joining two way-points with zero velocity and acceleration conditions [2]. Given a pre-defined task time, the maximum acceleration along the trajectory was minimized. MiillerKarger et al. also planned the trajectories in joint space but used a normalized hyperbolic function that was scaled to describe the desired trajectory and provided infinitely continuous trajectories [34]. This method also allowed the definition of a third way point that was used to avoid obstacles. Gosselin and Hadj-Messaoud proposed a solutions that would ensure C and C continuity at the initial and final points 2  3  using quintic and seventh order polynomials to join the initial and final configurations [25]. Frequently, two other way-points are defined in pick-and-place operations: a lift-off point and a set-down point so as to provide acceptable departure and arrival paths [35]. Gosselin and Hadj-Messaoud also proposed a solution for this case involving the use of a seventh order polynomial to link all four waypoints while ensuring zero velocity and acceleration trajectory end conditions and a continuous jerk trajectory [25]. A second solution involving a ninth order polynomial to ensure C continuity at the initial 3  and final points of the trajectory was also developed. Gosselin and Hadj-Messaoud found that the 7 and th  5  th  order polynomials were smooth monotonic functions, and therefore, the problem of undesirable  oscillations could be avoided. However, the 9 order polynomial was not always monotonic. If more wayth  points must be defined in order to avoid obstacles, a single function used to join all the way-points with acceptable end conditions will require higher order polynomials whose shapes are increasingly hard to predict since they depend on the placement of the intermediate way-points. All of these methods used normalized trajectory functions that were scaled to fit the desired way-points. This is possible since the start and end positions of the trajectories are given zero velocity and acceleration conditions and a single function is used to join all the way-points.  Chapter 2  13  2.4.2.2. Multiple Way-Points with Prescribed Path  Often a manipulator task is defined in terms of constrained motion: that is the manipulator end effector must follow a specific path. In this case, the pick-and-place methods described above can not be directly applied. These methods do not allow the specification of more than three to five way-points, which may not be sufficient to avoid obstacles. Specifying more than five way-points would involve the use of high order polynomials whose shapes are hard to control, perhaps resulting in retrograde motion. Planning in joint space is convenient since it avoids the computational burden of converting the trajectory points to joint space coordinates at every controller sampling interval, that would be necessary if the trajectory were planned in task space. However, it is more intuitive to specify way-points in task space. Thus, much work has been done involving converting task space way-point and intermediate point coordinates into joint space coordinates and planning the trajectory in joint space, resulting in approximate straight-line task space motion [11, 31, 47]. Taylor derived an algorithm that generates a sufficient number of intermediate points to guarantee that the deviation of the manipulator end effector from a desired straight-line path is within a prescribed tolerance when the trajectory is generated in joint space [47]. Taylor did not consider velocity or acceleration limitations. Lin et al. proposed an on-line method of generating a joint space trajectory through any number of way-points defined in task space [31]. The trajectory is planned joint by joint and the time to move between the way-points is equal to the controller sampling time. They also compare the use of X-spline and quartic spline trajectories, and found that the X-spline functions provide better path approximation (i.e. smaller path deviation) because continuity in acceleration is not enforced. Chang et al. proposed a similar on-line trajectory planning method, involving the approximation of the desired arbitrarily smooth task space path by a series of concatenated line segments intersecting at knot points [11]. Recursive joint trajectories are then derived by interpolating these knot points using quartic splines to ensure continuity of acceleration at the knot points and allow the acceleration along the segment to be minimized. The knot points are determined such that the resulting task space path deviation is small. Trajectory planning of a similar type has also been done in task space. Bazaz and Tondu used three cubic splines to join way-points in task space providing continuity of position, velocity and acceleration at the way-points by imposing zero acceleration end conditions [3]. They are able to provide straight-line and circular segment paths by controlling the way-point velocities. They developed normalized velocity and acceleration profiles which are then scaled to fulfill the user's requirements. This also allows the intermediate velocity and acceleration values that will occur during the motion to be predicted.  Chapter 2  14  2.4.2.3. Kinematic Limitations at, and Between, Way-Points  The methods discussed above only take into account the way-point positions, velocities and accelerations. They do not specify the velocity or acceleration profdes between the way-points. Instead only the acceleration between the points is minimized or only continuity in velocity is provided and, in certain cases, acceleration or jerk. However, it is important to ensure that the kinematic limits of,the machine are respected not only at the way-points but also between them so as to provide feasible motion and reduced actuator wear [5, 7, 30, 32, 52]. Bazaz and Tondu's work, described above, does allow for the prediction of the velocity and acceleration profde and therefore the motion time could be modified so as to stay within velocity and acceleration limits [3]. The shape of the position profile is also important. Overshoot of the desired positions frequently occurs when high order polynomials are used and is undesirable in most tasks [9]. For example, position overshoot in assembly operations may cause the manipulator to contact the surface of the workspace or the object that it is to pick up, or could cause uneven welds in a welding operations. A commonly used trajectory profile is the Linear Segments with Parabolic Blends (LSPB) [42]. It has the square wave acceleration profile shown in Figure 2.1(a). Fisher et al. provide a method of calculating the minimum time necessary for a motion based on coordinated joint motion (all joints reach the way-points at the same time) and limiting the velocity and acceleration of the LSPB motion [23]. In accordance with [4, 44], they found that for a minimum-time motion one joint is at its acceleration limit and another joint at its limiting velocity for all end effector motions. This is the time-optimal motion that could be obtained if actuators were able to change accelerations instantaneously. However, applying an LSPB profile to an actuator may introduce transients into the motion and excite structural vibrations, affecting path tracking and increasing actuator wear [9, 40].  I (a) LSPB Acceleration Profile  7  (b) Acceleration Profile Used by Castain and Paul [9]  Figure 2.1. Proposed Acceleration Profiles  (c) Acceleration Profile Used by Brock and Kaczmarek [5]  Chapter 2  15  Castain and Paul modified the LSPB profile by limiting the rise time of the waveform (Figure 2.1(b)). That is, rather than allow the profile to change acceleration instantaneously, changes in acceleration follow a ramp profile (i.e. a trapezoidal acceleration profile) [9]. The rise time was assigned to be a fixed proportion of the time necessary to move between the two points. They found that this profile, the cubicquadratic-cubic-squared profile, caused a 6.9% increase in motion time. All motions between two waypoints are described with a set of six splined cubic equations. The position and velocity at the two waypoints are specified. The trajectory is planned based on the initial and final point as well as an intermediate point determined by the algorithm. The accelerations at these three points are set to zero. The algorithm ensures that the acceleration limit is respected along the entire trajectory by calculating the planned accelerations and comparing them to the limit. If the acceleration limit is violated, the time is incremented and new accelerations are calculated. The algorithm can calculate a trajectory between any number of way-points and assumes that the first and last way-points provided have zero speed conditions. The resulting trajectories provided smooth motion and no overshoot or undershoot of the way-points. The main disadvantage of this method is that it requires iterative calculations to ensure that the acceleration limits are respected and does not take velocity limits into account. The algorithm assumes that the user provides the motion time or it uses a default time. Erkorkmaz also used a trapezoidal acceleration profile to provide a smooth motion that stayed within jerk, acceleration and velocity limits [20]. However, in contrast with Castain and Paul's method [9], the profile could be formed of up to seven segments (a zero acceleration segment was included). The method requires solving a nonlinear system of equations using Newton-Raphson's algorithm to obtain the segment times. The reference trajectory is generated with varying interpolation periods and therefore requires re-sampling at the control frequency. This re-sampling was done using quintic polynomials, with boundary conditions estimated using third order polynomials, to ensure the C continuity of the trajectory. 2  Brock and Kaczmarek reported that use of an LSPB profile for CNC machining caused contouring errors due to the system's inability to provide the required torque at the points where instantaneous changes in acceleration were required [5]. They suggested the use of a squared sine function to provide continuity of acceleration (Figure 2.1(c)). The squared sine function was used to ramp the acceleration whenever a change in acceleration was required. A typical reference profile was composed of seven segments: three constant acceleration segments and four squared sine function segments. The profile provided a suboptimal motion that they applied to curvilinear paths while staying within acceleration and velocity limits. The jerk limitation is provided by adjusting the acceleration ramp time.  Chapter 2  16  2.5. Summary of Trajectory Planning Techniques In summary, trajectory planning techniques have been developed to improve productivity and tracking accuracy. Trajectories that are purely time-optimal [4, 44] and based on the machine dynamics do not take into account actuator torque rate limitations and lead to controller saturation when implemented. This results in poor tracking accuracy and machine wear. Controller-based and planning-based solutions to this problem have been proposed [8, 14, 17, 27]. However, it is unlikely that the controller-based methods will be implemented in an industrial setting since they require complex control laws. In addition, trajectory planning methods that incorporate the machine dynamics cannot be used on-line since they represent a high computational load. Trajectory planning techniques based solely on the kinematic limits of the actuators have also been developed and can be implemented on-line. These methods are time suboptimal since the kinematic limits are determined experimentally and are for a "worst case scenario" [28]. Of most interest to the work described herein is the subset of kinematic methods that limits the acceleration, velocity and jerk during the entire trajectory between two way-points [5, 9, 20]. 2.6. Problem Formulation The objective of this research is to develop a method of generating smooth multi-axis machine trajectories, suitable for on-line applications. That is, trajectories that stay within velocity, acceleration and jerk limitations throughout the motion. The trajectories should be near time-optimal so as to increase the productivity of the machines. Acceleration and jerk limitations are considered in order to reduce wear and tear on the actuators, prolong machine life and reduce servicing. Tracking accuracy can also be improved by using smooth trajectories since the controller saturation is avoided, as was discussed in the preceding sections. The algorithm should be computationally simple for on-line implementation. Thus, in this work, the proposed algorithm is based on providing kinematic limitations to the motion (i.e. limiting the jerk, acceleration and velocity of the motion), rather than directly incorporating the full dynamic model of the machine. Although this results in underutilization of the machine's capabilities [4, 28, 41, 44], incorporating the dynamics of the machine would severely complicate the algorithm due to the inherent non-linearities, and prohibit use of the algorithm on-line. Further developments on this research could lead to the incorporation of linearized dynamics in the planning algorithms. However, this is not within the scope of this research.  Chapter 2  :  17  The basic trajectory planning algorithm should provide a time law that can be used on any parametrizable path, described in any space, provided that the corresponding jerk, acceleration and velocity limits are derived. Thus, such a trajectory could be used on different types of machines such as manipulators, CNC machines, cranes, etc., where a reference trajectory is used to describe the machine's task. This work is closest to that by Brock and Kaczmarek [5], Castain and Paul [9] and Erkorkmaz [20], described in Section 2.4.2.3. It ensures that the velocity, acceleration and jerk limits are respected along the entire trajectory and not just at the way-points (the entire profile must be defined). The trajectory must be C continuous and a desired speed, which should be attained if physically possible, will be given to 2  each way-point. Most methods described in literature do not allow for the specification of all these properties. As discussed previously, Erkorkmaz limited the jerk, acceleration and velocity of the motion. However, the method requires optimization to determine the motion time, and cannot be implemented on-line. While Castain and Paul limited velocity and acceleration, they did not incorporate jerk limitations. The method proposed by Brock and Kaczmarek is closest to the requirements of this work since it is possible to choose the time parameters in such a manner that the jerk limits are respected. However, the use of sine functions leads to computational complexity that should be avoided if the algorithm is to be used on-line. The algorithm developed herein was developed independently of Brock's work, but the results are quite similar. Furthermore, Brock and Kaczmarek only discuss the case where the maximum allowable velocity between two way-points is greater than the velocity at the start and the end way-points. As well, their method is applied only in two dimensions but can be used along cubic spline paths. The work presented herein also considers the case where the maximum way-point speed is the limiting path speed, as well as cases involving speed and distance limitations (more typical of manipulator trajectory planning than the CNC trajectory planning proposed by Brock and Kaczmarek). This work is applicable to motion in any dimensional space, addresses the issues related to joint space and task space planning, and provides a method for transiting past way-points with a specified approach distance. Another important consideration is that the resulting trajectory should not contain any oscillations in the position domain since undershoot or overshoot of way-points is often undesirable. For example, in the case of a manipulator, such oscillations may cause the manipulator to contact with an obstacle in the workspace. The manipulator may end up pushing a workpiece into the supporting surface or away from ' the manipulator causing damage to the manipulator or workpiece and/or failure of the task. This type of  Chapter 2  18  behaviour can be hard to control when polynomial trajectories are used to describe the trajectory [9]. Thus, the trajectory must be formed in such a manner that no such oscillations occur. 2.6.1. Implementation The second objective of this work is to implement the trajectory generation method for use with robotic manipulators based on guidelines provided by an industrial robot manufacturer [19]. The various issues related to this implementation are discussed below. 2.6.1.1. Task Space Planning Versus Joint Space Planning  In most cases, a manipulator path will be described in terms of task space coordinates. For applications such as gluing, arc welding, spraying or cutting, the specified path must be followed accurately. Therefore, it is necessary to plan the entire trajectory in task space rather than to approximate the path by interpolating knot points in joint space. Motions planned in task space also provide a motion that is predictable: that is, it is easy for the user to tell whether the manipulator end effector will contact an obstacle [47]. A path in joint space is not intuitively predictable. In addition, for certain applications such as carrying trays filled with fluids, keeping the end effector orientation constant during the motion may be necessary. This can easily be specified in task space, whereas a joint space motion does not provide such guarantees. Thus, the added computational burden of performing the inverse kinematics at every control interval is justifiable. However, the value of the ability to plan a task in joint space should not be overlooked. It has been shown that the fastest motion between two way-points approximates a straight line in joint space [41], Therefore, in areas of the workspace where no obstacles are present, it is desirable to move as fast as possible, and a straight-line joint-space path is the best option for on-line computation. In addition, motions planned in joint space will reach the joint space way-points with better accuracy since errors will not be introduced by the forward and inverse kinematics. Finally, following a straight-line path described in joint space will provide coordinated joint motion. Coordinated joint motion provides smooth well-behaved end effector motion that is path invariant with manipulator speed [23]. This provides predictable motion in systems where a path is taught to the manipulator at a low speed and the manipulator is to carry out the tasks at high speed. When the motion is run at high speeds, the robot will follow the same path it followed at the lower speed.  Therefore, a user can use joint space motions for gross motions when the manipulator is not carrying anything, and is simply transiting between two tasks (for example two pick-and-place operations). In  Chapter 2  19  areas where many obstacles are present, or when the manipulator is carrying something, straight-line motion in task space can be used to ensure that no obstacles are contacted and/or that the object the manipulator is carrying is not damaged. The ability to plan in either joint or task space is provided in this work. 2.6.1.2. Path-Related Constraints A straight-line motion provides a motion that is easily predictable from the user's point of view, simplifying task planning. However, if straight lines are used to connect the way-points, the manipulator must stop at each way-point due to the inability of its actuators to provide discontinuous velocity. Thus, it is common to connect the straight lines with curves that pass close to the way-point, but not necessarily through the way-point [33, 35, 50]. An industrial standard [19], followed in this work, is to blend within the angle defined by the two straight lines. This standard is followed in both task and joint space. It is necessary to ensure that the kinematic limits are respected during the curved portion of the motion. Straight-line motion can be provided by superimposing the trajectory profile on the parametrized straight, line described in either joint space or task space. However, task space planning possesses the additional complexity of planning the end effector orientation trajectory. Orientation cannot be treated as a vector, and smoothed Euler angles do not necessarily provide a smooth motion [22]. Thus, a method of providing smooth orientation interpolation between the way-points is presented in Chapter 5. 2.6.1.3. High Level Planner As defined by Bazaz and Tondu, on-line trajectory generation is the computation of the next trajectory segment while the manipulator is executing the present trajectory segment [3]. An overall planning algorithm is needed to coordinate the on-line trajectory planning for multiple way-points. The high level planner obtains future way-points from the scheduler, and then calls lower level functions to calculate trajectories based on the type of motion the user requires (e.g. straight line, straight line followed by a blend, etc.). It is desirable that over a planned window of N way-points, the user-specified speeds (expressed as a percentage of maximum speed) be attained at each way-point, if physically feasible. Furthermore, the trajectory must always have a stop condition at the end of the planning window in case no new way-points are provided. If new way-points are provided, the imposed stop condition can be removed. In this manner, it is not necessary to stop at every way-point, and a significant speed can be reached along an extended trajectory. The choice of the number of points in the window will require a trade-off between the overall motion time and the amount of time the robot will delay (for planning) until  Chapter 2  20  it starts the motion. As well, the larger the planning window, the greater the buffer available to handle trajectories with a large number of closely spaced way-points with high speed demands. 2.7. Summary Purely time-optimal trajectories require bang-bang torque profdes, which are unattainable by the actuators due to physical limitations. Such trajectories result in controller saturation which reduces tracking performance and induces vibrations in the machine [14]. Trajectory generation algorithms, which take the torque rate limitations into account and use the dynamic model of the manipulator, provide smooth motion that the actuators can track more accurately and reduce wear in the machines. However, such algorithms rely on optimization of non-linear multiple degree of freedom equations to calculate the trajectory and are not practical for on-line industrial manipulator trajectory planning. An alternative is to consider only the kinematic limits of the actuators obtained from the dynamic models of the machines (velocity, acceleration and jerk) and to plan the trajectory based on these limits. The constant bounds on these variables are chosen as the effective lower bounds in the entire workspace. This results in an underutilization of the machine capabilities since the task space limits are determined experimentally and are for a "worst case scenario" [28]. However, such methods can be successfully implemented on-line. Existing methods in the literature that limit the jerk during the entire trajectory (i.e., smooth trajectories) generally require iteration, optimization or frequent calculation of transcendentals and are not suitable for on-line implementation.  The present research provides a method of generating near time-optimal smooth trajectories that can be applied on-line. To the author's knowledge, this is the first-reported industrially-implemented, on-line, jerk-limited motion planner. The trajectories can be planned in task or joint space and are composed of straight-line segments connected with blends. Furthermore, an overall planning algorithm used to generate trajectories between multiple way-points was developed. This work has been implemented on a robotic manipulator, and experimental results demonstrating the utility of the approach will be presented. It will also be shown that the on-line method developed herein compares favourably to a time-optimal with torque rate limitations method, and provides a substantial improvement over a typical on-line motion planning approach.  Chapter 3 Methodology In this chapter, the methodology used to construct an efficient, jerk limited, parametrized, onedimensional trajectory between two way-points is outlined. The trajectory can then be extended to any parametrizable path, as will be shown in Chapter 5. The method has low computational complexity and, therefore, can be applied on-line. As outlined in Chapter 2, an important step towards fulfilling the objectives of this research is the generation of trajectories for which speed, acceleration and jerk are limited while travelling between two specified way-points in either cartesian or joint space. The lowest order polynomial function which respects these kinematic limits at every trajectory way-point is a seventh-order polynomial function. Such a function would allow the specification of eight end conditions (i.e. initial and final: position, speed, acceleration and jerk). However, simply connecting any two way-points with such a polynomial does not guarantee that the kinematic limits are respected at all points between the two way-points. For example, the polynomial may cause retrograde motion or overshoot of the speed, acceleration and/or jerk limits [9, 10, 37]. It will be shown in this chapter, that by using a concatenation of fifth-order polynomials and a sine wave template, it is possible to construct an efficient trajectory between two way-points that respects all the required kinematic limits. Section 3.1 provides the basis function used when a change in acceleration is required, and provides two different algorithms used to obtain changes in speed along a trajectory: the sustained acceleration pulse algorithm and the acceleration pulse algorithm. These algorithms are then used in Section 3.2 to generate trajectories between any two one-dimensionalized way-points; that is, the trajectory is parametrized along a straight line between any two multi-dimensional way-points. 3.1. Trajectory Functions (Building Blocks) Linear segment with parabolic blends (LSPB) trajectories are commonly used by industrial manipulators and machine tool trajectories for their fast motion and low computational complexity. LSPB trajectories switch between maximum (a ), minimum and zero accelerations to achieve time-optimal motion while mm  respecting imposed speed limits (s ) (Figure 3.1). However, this type of motion has infinite jerk, and as 21 max  Chapter 3  22  a consequence, is not easily tracked, resulting in a longer motion time than expected and deviations from the prescribed path. If LSPB motion could be approximated using a series of smooth functions that are at least C continuous, that is, continuous in their second derivative, the resulting motion would be fast and 2  trackable.  10001  k  .  1  0  0  0  l 0  0.5  ,  1  ,  J  1  1.5  2  2.5  time [s]  Figure 3.1. Linear Segment with Parabolic Blend (LSPB) Trajectory Between Way-Points pi andp * 2  Fifth-order polynomials (quintics) are the lowest order polynomials for which it is possible to specify the end point positions, speeds and accelerations (Eq. 3.1), g(t) = b t +b t +6 / +b t +b t + b , 5  4  5  4  3  3  2  2  i  0  (3.1)  where t is the time. This polynomial provides a quadratic jerk profile, as opposed to a cubic polynomial, which provides a constant jerk profile. At a minimum, a cubic trajectory will result in jerk discontinuities, and potentially, infinite jerk, at trajectory way-points. Andersson used quintics for stop-point-to-stop-point motion, and derived the quintic motion time such that speed and acceleration limits are respected [1]. Using this method, it is also possible to ensure that jerk limits are respected. However, extending this work to cases where the initial and/or final speeds are non-zero would require the computational expense of repeatedly solving a third-order equation. Furthermore, the resulting trajectory will be slower than the one proposed herein and may have a number of undesirable oscillations in position, velocity and acceleration between the way-points.  * The maximum occurring jerk of 1000 m/s is a result of the sampling time interval of 1 ms used to generate the 3  plots. The maximum jerk seen by the system will be a function of the controller sampling interval.  By assigning appropriate start and end conditions to the quintics, a smooth trajectory joining two specified way-points can be found, while satisfying acceleration continuity (limited jerk) at the trajectory end points. That is, a trajectory between two way-points is formed as a series of linked quintics, with each quintic having initial conditions equal to the end conditions of the previous quintic. Such a trajectory would have C continuity. The intermediate points between the linked quintics are called "quintic control 2  points". In fact, the entire trajectory can be characterized by a series of these control points. Therefore, during the trajectory planning stage, it is only necessary to calculate the quintic control points corresponding to the trajectory. This provides a computationally simple and rapid method of planning the trajectories between a series of way-points, as described in Chapters 5 and 6. Between the way-points, it is desirable to have the quintics follow a LSPB trajectory template without undesirable oscillations. This requires careful selection of the quintic control points such that each quintic will behave in a prescribed manner, respecting speed, acceleration and jerk limits. The LSPB pattern is used to provide the "switching" points for the trajectory. That is, the trajectory is still formed of a series of speed ramp and speed cruise segments, like the LSPB in Figure 3.1. However, in this work, changes in acceleration are generated using sine-shaped ramps, rather than the abrupt square wave signals that generate infinite jerk. The sine wave template provides a constant relationship between its maximum amplitude and the maximum value of its derivative. Selecting the time for acceleration and deceleration ramps based on a sine wave with appropriate properties, allows the jerk limits to be respected. Furthermore, the sine shape is  infinitely differentiable and provides a highly smooth trajectory  profiles. The resulting  acceleration/deceleration ramp is, therefore, smooth and there is no positional oscillation. 3.1.1. Sine wave template As stated above, a change in acceleration is accomplished by following a sine wave template (Figure 3.2). A single quintic is used to approximate this motion. As will be shown, using a quintic to approximate the shape allows fast computation of the desired trajectory by avoiding both computation of transcendentals, and trajectory scaling. Since the end conditions are specified (initial and final: position, velocity and acceleration), the time allotted for the change in acceleration is the only free variable. This variable is used to ensure that the jerk limit is not violated.  Chapter 3  24  1.2  0  0.1  0.2  0.3  0.4 0.5 time [s]  ,  0.6  0.7  0.8  Figure 3.2. Sine Wave Template for a Positive Change in Acceleration Between Zero Acceleration and the Acceleration Limit, a max  By setting the amplitude of the sine wave equal to half the acceleration limit, a /2, and the maximum max  slope equal to j , the maximum jerk, the minimum time that should be allotted for the acceleration ramp max  motion can be derived. The acceleration equation for the sine wave motion can be written as:  where t is the time and dt  max  (3.2)  t  -sin  is the total time allowed for the motion. Taking the derivative of Eq. 3.2 with  respect to time gives the equation for the jerk, IT  f  Ji(t)= 2dt m,ma  K  cos  max  max  \  t  K  ^  (3.3)  •j  The maximum slope of the acceleration sine wave profile occurs at t = 0.5dt . Setting Eq. 3.3 equal to max  j  max  at t = 0.5dt  max  provides the equation for dt„ , wx  dt max  =^as-. .  J  (3.4) \  max  y /  Chapter 3  25  Integrating Eq. 3.2 provides the quintic speed and position end conditions corresponding to the sine wave template in Figure 3.2. The speed end conditions for the quintic are *(-) = -,,  (3.5) (3.6)  + s, , where s\ is the initial velocity. The position end conditions are  (3.7)  p(0)=P , t  P{  d t  ma )= max a  X  d t  2 max  _i_ 4  i_ 7t  (3.8)  2  wherep is the initial position. The acceleration end conditions are already specified as x  a(0)  (3.9)  = 0,  (3.10)  ct(dt ) ~ ci . max  max  A deceleration profile from can be derived in the same manner. When a quintic with the end conditions developed above is used, the acceleration is limited to the interval [-cimax, +a x] ma  and the jerk to the interval [-0.98 j ,  +0.98 j ]. The motion also has no substantial  max  max  retrograde or oscillatory effects in the position and velocity domains. The jerk cannot be specified at the way-points, and therefore, the jerk profile is limited and has a limited discontinuity. It can be shown that by using the sine-wave template, the magnitude of this discontinuity is at most 11% of the jerk limit (Appendix A). 3.1.2. Non-Limited Trajectory In Figure 3.3, the construction of a smooth trajectory between two way-points (/>>, s\) and (p , s ), using a 2  2  concatenation of quintics and the sine wave approximation is illustrated. In this example, the change in position required to accelerate from speed si to speed s is smaller than the distance between the two way2  points. Thus in this case, the trajectory reaches the maximum speed, s . This example represents the 2  simplest case for a transition between two way-points. In Figure 3.3, p\ and p are, respectively, the initial and final positions, and s\ and s are the initial and 2  2  final speeds. The speed reached at the end of the ramp from zero acceleration to the maximum acceleration, a , is s . The time taken to ramp up to a„ max  a  mx  is dt . The speed reached at the end of the max  Chapter 3 acceleration cruise at a  26  max  is Sf,. The maximum allowable jerk is j . The acceleration ramp up to a max  max  is  followed by an acceleration cruise and then an acceleration ramp down to zero. The ramp down begins when the remaining required change in speed is equal to ds i, where, ri  a ds = rd  dt max  .  (3.11)  The acceleration ramp up and ramp down are symmetrical, simplifying the integration process. The speed ramp is also symmetrical about a straight line drawn from the start of the ramp to the end of the ramp.  time [s]  Figure 3.3. Smooth Trajectory Generated with a Sequence of Quintics (• indicate the quintic control points) The corresponding LSPB trajectory time (Figure 3.1) for the same speed and position change is smaller than the quintic trajectory time (by dt ), however, the quintic trajectory has bounded jerk, and is max  therefore trackable. The quintic control points corresponding to the speed ramp are determined as specified in the Sustained Acceleration Pulse (SAP) Algorithm below. A deceleration trajectory can be derived in the same manner by reversing the end point speeds, planning the corresponding trajectory and then making the appropriate changes to the control points (Section 3.2).  Sustained Acceleration Pulse Algorithm (cf. Figure 3.3) (s? > Si > 0) 1.  Calculate the total way-point to way-point distance, D = \p — P\\ • 2  2. Calculate the two unknown control point speeds s , a  ® max ^  max  d  (3.12)  and s , h  S /,  3.  —  a  5 ->  dt max  (3.13)  max  Calculate the distances covered during: - the ramp up to a„ , mx  D\  + \ K\ax S d  ~ ma dt x a  X  ma  4  K  (3.14)  •  1  the acceleration cruise, D =2  the ramp down from a  nmx  to zero acceleration  ~ max^max  ^3  4.  IF  (3.15)  2a „  a  1 1 ^ -++ 14 K>  S  h  d  t  (3.16)  max  {D,+D +D -D>Q) 1  J  It is not possible to reach the demanded speed, s , due to the distance, acceleration and jerk limitations. 2  Therefore, reduce s and s so that the distance constraint is respected (i.e., reduce the length of the h  2  + ,  («max maxf  acceleration cruise):  t  ,  2 ,  @max®'max S  h  = -  a m  a x  d  t  m a x  ~  dt  c  l  n  i*.  _  2a.,  amax „ dt, max m  s =s +2  2  D  (3.17)  (3.18)  r  b  Recalculate D and D from Eqs. 3.15 and 3.16. 2  5.  3  Assign the times, positions (using D D , and D ), speeds (s s„, Sh and s ) and accelerations (zero or a ) h  2  3  h  2  max  corresponding to the four quintic control points that form the non-zero acceleration portion of the motion (refer to Section B. 1, Appendix B).  To generate the entire trajectory depicted in Figure 3.3, the first four quintic control points are calculated based on the SAP Algorithm. A fifth quintic control point corresponding to the end of the speed cruise is  Chapter 3  28  also necessary. In Section B . l , Appendix B, the control points conditions for the entire way-point to waypoint trajectory can be found.  3.1.3. Limited Trajectories For a short travel distance, D, between the two way-points pi and p , an acceleration pulse of a 2  max  may  result in an overshoot of the distance D. This is the case when  D<D ,  (3.19)  mln  where D  mln  + 2s dt  = a dtl max  wx  x  (3.20)  max  Again s is assumed to be larger than si. Furthermore, if the desired change in speed, ds = (s — s. )is such 2  2  that ds<ds , min  (3.21)  mm= max maxi  (- )  where dS  then an acceleration pulse of a  max  dt  a  3  22  will overshoot the desired speed change, ds.  In both cases, the maximum allowable acceleration, a , cannot be reached without overshooting a max  prescribed trajectory limit (D or ds). In such cases, the SAP algorithm described in Section 3.1.2 must be modified. The motion is now based on an acceleration ramp up to a lower acceleration, a k, and then an pea  immediate ramp down to zero acceleration, i.e. an acceleration pulse of a k. If the motion is limited by pea  the change in speed, the acceleration pulse may be followed by a speed cruise.  If D < D  min  and ds > ds , then only the travel distance limits the motion. In this case, the equation mjn  describing the distance, D, based on an acceleration pulse, can be found through integration of the jerk, acceleration and velocity profiles: D = U^dt  3  + 2 dt,  (3.23)  S]  K  where dt is the time necessary to ramp up to a . peak  In this case, dt can be calculated by solving Eq. 3.23: ,3 _^l_ __^L  rf  +  =  A  max  0  i  (3.24)  Chapter 3  29  For si > 0, Eq. 3.24 has only one positive real root in dt, which can be found directly (Appendix C) [38] or via a Taylor series approximation in an average of 1.5 iterations. The peak acceleration, a k, is then pea  2j dt ^  If D>D  m!n  and ds<ds , min  necessary to ramp up to a  peak  J max  peak  71  (3.25)  then only the desired change in speed is limiting. In this case, the time can be calculated from: dt =  \n(s -s ) 2  x  ^"J max  (3.26)  In the case of a motion limited by distance or speed, up to three quintics are necessary to describe the motion between two way-points: the acceleration ramp up quintic, the acceleration ramp down quintic, and, in the case of limitation by speed, the speed cruise quintic. A sample trajectory for the speed limited case is shown in Figure 3.4.The algorithm used to determine the quintic control points for the speed or distance limited case is given below. It can be shown that this algorithm converges to the Sustained Acceleration Pulse Algorithm at D = D „. mi  0  0.5  1  1.5  Figure 3.4. Speed Limited Trajectory (• indicate the quintic control points)  Chapter 3  30  :  Acceleration Pulse Algorithm fs? > Sj > 0) 1.  l¥(ds<ds „&D<D „) mi  mi  Calculate dt from Eq. 3.24, and a  from Eq. 3.25.  peak  IF (ds < dpeakdt)  Recalculate dt from Eq. 3.26 and a  peak  from Eq. 3.25.  ELSE IF(D < £>„„„) Calculate dt from Eq. 3.24, and a  peak  from Eq. 3.25.  ELSE IF(cis < ds ) min  Calculate dt from Eq. 3.26 and a  peak  2.  from Eq. 3.25.  Assign the time, position, speed and acceleration corresponding to the three quintic control points that form the acceleration pulse portion of the trajectory.  To generate the entire trajectory depicted in Figure 3.4, the first three quintic control points are calculated based on the Acceleration Pulse Algorithm. A fourth quintic control point (corresponding to the end of the speed cruise phase) is assigned based on the remaining distance: dD = D-(s +s )dt. 2  (3.27)  l  The quintic control point conditions for this case can be found in Section B.2, Appendix B. 3.2. General Trajectory Generation Between Any Two Way-Points  The algorithms described in Sections 3.1.2 and 3.1.3 can be used to determine a trajectory between any two way-points. Each way-point has a specified position and speed, while the acceleration at each of these way-points is set to zero to ensure continuity in acceleration and reduce the computational burden. Herein, these trajectories are used along straight-line paths: p(t)=P^^\ {t),  (3-28)  q  where pi and p are respectively the positions of way-point (1) and way-point (2) and q(t) is the trajectory 2  generated using the control points described in this section. q(t) starts at zero and ends at \p - p ||. Thus, 2  x  a trajectory linking any two given way-points is calculated based on the distance between the points, the speeds that must be achieved at each of the way-points, as well as the acceleration and jerk limits. It should be noted that p and p may not be the scheduler-defined way-points, but rather the points situated s  2  at the end and the start of a blend, as described in Chapter 5.  Chapter 3  31  In the following cases, D is the distance between the two way-points, point (1) and point (2); s\, s are the 2  desired speeds at points (1) and (2) respectively; and the speed limit, s  , is the highest allowable speed  max  when travelling between points (1) and (2). Thus trajectory is  If s <S\, 2  s  s  max  > (s, ,s ). The maximum speed reached during the 2  .  peak  the speeds are interchanged such that all motions can be treated as speed increases from Si to s . 2  The appropriate modifications are then made at the end of the trajectory calculations (Section B.3, Appendix B). This allows all motions to be separated into two categories based on speed. In category A , where s = s 2  , the maximum path speed is limited by the user-defined limit, s . This category also  max  2  includes motions that are limited by distance or speed. In category B, where s < s 2  , the maximum path  max  speed is greater than that allowed at point (2). Figure 3.5 is the flowchart corresponding to the general algorithm outlined below.  constant speed trajectory  0 J  1  $ max  i  y  *x  $2  Acceleration Pulse Algorithm (Section 3.1.3)  Sustained Acceleration Pulse Algorithm (Section 3.1.2)  11a  B iii Figure 3.5. Flowchart Indicating the Algorithm to Use Based on the Way-Point Speeds  Chapter 3  ;  Case A. s  max  32  = S2  If S2 = Si, a constant speed trajectory can be used. The coefficients of Eq. 3.1, b , b , b , b are set equal to s  4  3  2  zero, and Eq. 3.1 becomes afirst-orderpolynomial. Otherwise, in order for the motion to be carried out as rapidly as possible, the motion begins with an acceleration phase: IF D > D  min  (Eq. 3.20) & s -s, > ds „ (Eq. 3.22), 2  mi  The trajectory outlined in Section 3.1.2 (with a sustained acceleration pulse) is used. ELSE The trajectory outlined in Section 3.1.3 (with an acceleration pulse) is used. Case B. s  max  >s  2  Several subcases are possible based on the distance, D, and the speeds s s , s : h  2  max  i. IF the distance necessary to ramp from si to s , D , is greater than D, 2  reduce s  max  s  max  such that s  -s > ds , 2  min  max  where ds  min  v  = s . Go to Case A. 2  is calculated from Eq. 3.22.  A minimum distance, D ,, is calculated. D limi  is the minimum distance necessary to be able to use  limit  the sustained acceleration pulse algorithm to calculate trajectories when ramping up from si to s/,„„, and then downfrom£/,„,„ to s (Figure 3.6). s/, „ is the speed reached by an acceleration pulse from s 2  m  2  at Cl '. max  s  ,=a dt +s,. (3.29) limit max max 2 v > The calculation of Du u, is based on integration of the acceleration profile shown in Figure 3.6: r  m  D,  =D +D,  imit  a  (3.30)  b  where D = D +D +D a  la  2a  (3.31)  3a  is the distance covered during the sustained acceleration pulse used to ramp up from Si to s/,„,„ (refer to SAP algorithm, Section 3.1.2), and D = a dt  2  h  max  max  + 2s dt 2  max  = (s  limil  (3.32)  + s )dt 2  max  is the distance covered during the acceleration pulse used to ramp down from s to 5/,„,„. 2  Dumu is compared to the required distance, D, to determine whether the planned trajectory will be composed of acceleration pulses or sustained acceleration pulses.  Chapter 3  33  0  0.5  1  1.5  2  2.5  3  3.5  4  4.5  5  time [s]  Figure 3.6. Illustration of Dn , (• indicate the quintic control points) mi  ii a. D> Dn i, - Use sustained acceleration pulses. m  Sustained acceleration pulses are used to generate speed ramps from s\ to s  k  pea  and then from s „k pe  t O S2-  Let D  z  Z)  x  be the distance covered by a ramp from s to s t  be the distance covered by a ramp from s to s 2  max  max  (using sustained acceleration pulse) and  (using sustained acceleration pulse).  IF D > D +D x  z  s eak - Smax, P  and & speed cruise at s  max  is necessary.  ELSE Speak  < s , and the acceleration cruises corresponding to the speed ramps to and from s max  max  are shortened based on D, as shown by the dotted line in Figure 3.7, and explained in Appendix D.  Chapter 3  34  0  1  2  4  3  5  time [s]  Figure 3.7. Illustration of Shortened Acceleration Segments ii b. D< Dii„,i, - Use at least one acceleration pulse. In this case, it can be shown that s „k < s . s k is based on a speed ramp from s\ to s , followed pe  by a speed pulse from s to s 2  peak  nuix  pea  2  and back down to s , as shown in Figure 3.8. This calculated value 2  for s k is then used to generate the actual trajectory, consisting of a speed ramp from s to s , a pea  t  speed cruise at s  peak  peak  followed by a speed ramp from s k to s . pea  2  This calculation provides an efficient solution to this case over a more computationally intense (fourth order equation) solution for an optimal s k subject to the constraints of S\, s , D, a , and pea  Jmax-  |  Figure 3.8. Template Used to Determine s  time [s]  k  in Subcase iib.  2  max  Chapter 3 iii.  s  max  _35 -s < ds „ (Eq. 3.22) 2  mi  In this case, an acceleration pulse is needed for the ramp down from limiting distance is calculated, this time based on  distance to ramp required to ramp from si to  to s . As in subcase ii, a 2  max  peu  2  pea  =s .  s k  Let D be the distance required to ramp from s to s w  s k  peak  s k pea  (using an acceleration pulse) and D be the y  (using an acceleration pulse or a sustained  acceleration pulse). IF D > Av + A s eak  =  P  s . A speed ramp is used to reach s max  k  pea  from s\, and a speed cruise segment at  s k  is  s k,  if  pea  added. ELSE The same approach described in subcase Ub above is used. Thus for Case B, the trajectory is composed of a speed ramp from s to {  s „k, pe  a speed cruise at  pea  necessary, followed by a speed ramp from s k down to s . A maximum of seven quintics can be used to pea  2  describe the trajectory linking the two way-points. 3.3. Summary In this chapter, a method of generating a trajectory between two way-points, based on a one dimensional parametrization of the straight line connecting the two points, is outlined. This trajectory is obtained as a concatenation of up to seven quintic segments (eight quintic control points). A sine-wave template is used when switching between zero and non-zero accelerations to ensure that the trajectory is smooth and that the jerk limit is reached. The trajectory is near-time-optimal since it closely follows a LSPB trajectory and utilizes the maximum allowable accelerations and jerks, as would be provided by the manufacturer, whenever possible. The resulting trajectory has the desired end conditions (in as much as is possible), and the kinematic limits such as maximum acceleration, jerk and velocity are respected throughout the trajectory. The trajectory displays no retrograde motion and is computationally simple to plan. It can be used to define the time-law along any parametrizable path, as will be shown in Chapter 5.  As shown in simulation and experimental results in Chapter 6, the trajectories generated are faster than typical industrial trajectories without requiring a high computational burden. They also compare favourably with the computationally expensive jerk-limited time-optimal motion [14].  Chapter 4 Blends In the previous chapter, a smooth trajectory generation algorithm between two one-dimensional waypoints, while respecting the path kinematic limits, was described. However, it is necessary, in many cases, to be able to plan a smooth trajectory through a number of n-dimensional way-points that describe the desired path. If the point-to-point (straight-line) trajectory must pass exactly through every way-point, then the trajectory must halt at each way-point where the direction of the path changes. Otherwise, there will be discontinuities in the velocity profile of the trajectory. However, frequent stops at way-points slow down the overall motion. The speed that can be attained between the way-points is limited, and the overall average speed through a series of way-points is greatly reduced. These stops can be avoided by allowing the path to deviate slightly from the way-points, using blends to smoothly travel near the point while changing directions but not stopping [33]. The blending function, developed by Lloyd and Hayward for generating smooth transitions between two polynomial trajectories, was chosen, since it provides C continuity during the blend [33]. In this work, 2  this method is used to provide a smooth transition between any two straight-line segments that are predefined in n-dimensional space. Straight line paths have the benefit of null normal acceleration and jerk components: all velocity, acceleration and jerk components are tangent to the path and are equivalent to the trajectory velocity, acceleration and jerk. During the blends, the normal components are no longer null, and must be considered, as will be discussed in Section 4.2.3. Lloyd and Hayward's blend methodology is extended herein to incorporate acceleration and jerk limits on the blended trajectory. Apart from blended paths, the details of generating curved paths are outside the scope of this thesis. In Section 4.1, a brief introduction to the blends defined by Lloyd and Hayward is given. Then in Section 4.2, a subset of blends, namely geometric blends, is derived, and the method for limiting the acceleration and jerk during these blends is described. These blends are used in conjunction with the quintic 36  Chapter 4  37  concatenation trajectories of Chapter 3. Together they provide a trajectory with C continuity that passes 2  within a specified distance of the given trajectory way-points.  4.1. Lloyd and Hay ward's Blending Function In Lloyd and Hayward [33], a blend is used to smoothly connect two trajectory polynomials xi(i) and X2(t). It is assumed that xi(t) and x (t) are also smooth functions. Let the blend start at point p, and end at 2  a  point pn, (Figure 4.1). These points have corresponding velocities v and v . lh  /o  x'  i i f ' — ! — X — T—  — X—  t  w  t  s  Figure 4.1. Blend in Time Domain [33]  The blend between the two polynomials x/(t) and x (t) is designed in parametrized space with a new non2  dimensional time coordinate, C T - - i = ^ ,  (4.1)  such that the blend occurs in the interval o~ e [0,1]. In Eq. 4.1 t is the time parameter, / is the time at s  which the trajectories intersect and r is half the time allowed for switching between the two paths (Figure 4.1). Lloyd and Hayward approximate the two polynomials by linear paths during the transition interval, such that: x {(j) = b + m (T,  (4.2)  x (cx) = b + m a.  (4.3)  ]  2  ]  ]  2  2  where b,, nti are respectively the position at o"= 0 and slope of x,(d), and b , m are respectively the 2  2  position at a = 0 and slope of x (<7). Xi(ti) and x (d) are intersecting paths defined within the interval 2  ae [0,1].  2  Chapter 4  38  To ensure that the transitions between x/, x and the blend are smooth, the blend polynomial, x(o), must 2  satisfy the following boundary conditions :  *(°)=Pl<,»  (4.4)  ^( )=Pl6» 1  x(0)=2tv , la  x{\)=2rv ,  (4.5)  xi\)=Ar a  (4.6)  xh  x(0) = 4r a, , 2  2  a  ih  where a , is the acceleration at the start of the blend and an, is the acceleration at the end of the blend. /(  A quintic polynomial could be used to generate a blend satisfying the above conditions. However, the quintic does not control the shape of the blend (that is, the path that it follows). To address this problem, Lloyd and Hayward blend x^t) and x (t) together using a convex average of the two polynomials. In 2  addition, the average acceleration is minimized by adding a damping function, p\d), that does not affect the end conditions of the blend. The blend function is defined as : xia) = x, ( < T ) + a{a){x (a)-x. (<r))- Kp{a)v , 2  where  d  (4.7)  a{a) = 6<7 -15<7 + lOcr ,  (4.8)  /?(<r) = < T - 3 ( T + 3 c r - c r ,  (4.9)  5  4  6  3  5  4  3  v, =m -m.,  (4.10)  2  and Kis the parameter that controls the amount of acceleration compensation to apply. The resulting blend function is a sixth-order polynomial, if x (t) and JC (0 are first-order polynomials. t  2  4.2. Geometric Blends Herein, Lloyd and Hayward's function is used for the case when the straight-line paths between waypoints are not changing with time. That is, they are known in advance, and the start and end position of the blends are given through the definition of a variable, tightness, tightness is a measure of how closely the trajectory must approach a given way-point before blending to the next way-point trajectory can begin. This measure is important for industrial applications where the user will typically define the path as a series of straight-line segments between way-points. The tightness signifies the importance of not deviating from the straight-line path; to avoid collisions with other objects, for example. A large tightness allows the manipulator to quickly pass through a series of points by "cutting" the corners at the waypoints. A small tightness distance indicates that following the straight-line path is of higher importance than quickly travelling past the way-points. Zero tightness indicates that unless the adjoining paths are colinear, the trajectory must stop at the intermediary way-point (to avoid a discontinuous velocity profile). In this case, the manipulator will follow a series of straight line paths (Figure 4.2).  Chapter 4  39  In addition, the properties of the blends are set such that the blend is symmetrical about its midpoint. This provides the ability to limit the jerk and acceleration during the blend. Hereafter, such a blend is referred to as a geometric blend. The blends are used to join intersecting straight line segments. The condition of C continuity during the 2  entire trajectory, constructed of straight line segments, with zero acceleration end conditions, connected to blends, is respected, since the blends are sixth order polynomials that start and end with zero acceleration. 4.2.1. Conditions Necessary for a Geometric Blend In the following discussion a 2D case is considered. However, the method is easily extended into ndimensions as shown in Subsection 4.2.4. Figure 4.3 shows the geometry of a 2D blend that is used to connect two intersecting straight lines />,-p and p -p . 0  2  {  A distance, tightness, is provided by the  user, to indicate how closely the point being blended around, p\, must be approached. That is, \\P~ P i J = \\P~ Pi»|| = tightness, P.* = P>  ~ °]tightness P  ,  (4.11) (4.12)  \\P-P4  and Pu, = />, + [ ~ }tightness \\P2-P4 Pl  Pl  ,  (4.13)  where p/ is the position at the start of the blend and pn, is the position at the end of the blend. a  In a 2D case, the distance tightness describes a circle, centred at p The blend must lie within the circle. h  According to the industrial convention followed (cf. Section 2.6.2.2), the blend must be located within the isoceles triangle with vertices p\„, p\ andpw,. Planning a motion that lies outside this triangle may result in collision with other objects. Eq. 4.11 ensures that the paths intersect at 0 = 0.5.  Chapter 4  40  The two polynomial paths between which the blend is used are first-order. That is, the linearity assumption in Eqs. 4.2 and 4.3 is exact (i.e. the start and end accelerations are null). Thus, the variables in Eqs. 4.2 and 4.3 are: *.=/>.«»  b =p -2rv ,  (4.14)  m =2Tv .  (4.15)  2  «,=2rv , l a  u  lh  2  lh  Figure 4.3. 2D Blend Geometry Another requirement of a geometric blend is that the blend incoming velocity, v , must lie along the line /o  between p and pi, and the blend outgoing velocity, V/A, must be colinear with the line from pi to p . In 0  2  addition, the incoming and outgoing speeds, s  and s/b, must be equal (si„ = si = s) so that the  /fl  h  acceleration and jerk during the blend can be limited. This leads to the following definitions of the incoming and outgoing blend velocities :  V  V)  \ a  15  (4.16)  || '  II  \\P-P>a\\  (4.17)  Given that p - p t  hi  = p- p t  ih  , and the two lines Xi(o) andx(a) intersect at cr= 0.5, 2  b + 0.5m =/> + 0 . 5 / M l  i  2  2  (4.18)  Chapter 4  41  Substituting Eqs. 4.11, 4.16 and 4.17 into Eq. 4.18, gives the equation for the half time allowed for the blend: T =  tightness  (4.19)  s  Since the two paths Xj and x are linear and intersect at o~ = 0.5, the optimal value of the acceleration 2  compensation parameter, K = 1 5 / 2 , is used (suggested by Lloyd and Hayward). This ensures that the blend will remain within the isoceles triangle with vertices p/ ,Pi and piba  A blend that has all the characteristics listed in this section has a fixed relationship between the maximum acceleration and jerk and the incoming/outgoing blend speed, and allows these variables to be limited as described in Section 4.2.3. It can be shown that for a blend that is not "geometric", the limiting equations for acceleration and jerk cannot be solved in a tractable analytical manner, although a numerical method could be applied. The velocities at the end of the straight-line trajectories correspond to those at the start and the end of the geometric blend. In addition, when the quintic concatenation method is used to describe the straight line trajectories, the trajectories have zero acceleration end conditions and C continuity. The geometric blend 2  function also has zero acceleration end conditions and C continuity. Thus, the geometric blend can be 2  used to join straight line trajectory segments, maintaining C continuity along the entire trajectory. 2  Herein, the geometric blend is used and the methods described in Section 4.2.3 are employed to limit the acceleration and jerk during the blend. 4.2.2. Velocity, Acceleration and Jerk Profiles for a 2D Geometric Blend In this section, an overview of the magnitude and individual components of the blend velocity, acceleration and jerk profiles is presented. The objective is to provide insight into the manner in which the • acceleration and jerk are limited. The velocity profiles for the x and v dimensions and the velocity magnitude in the o"-domain are shown in Figure 4.4 for the 2D blend example presented in Figure 4.3. Conversion to the time domain simply scales the profiles by a factor of 0.5 f'. In this case, the velocity in the x-direction decreases slightly, while the velocity in the y-direction has a correspondingly slight increase in magnitude but also a change in sign, which is consistent with the change in path direction of Figure 4.3. It can be seen that the speeds  Chapter 4  42  at the start and the end of the blend are equal. The speed decreases during the blend; in fact, in all cases during a geometric blend the imposed speed limit will be respected. The acceleration profiles (Figure 4.5) have two local maxima (if the absolute values are considered). The profiles are symmetric about the line o~= 0.5. As is expected, the v-coordinate acceleration magnitude is much large than that of the x-coordinate. The largest jerks occur at the start and end of the blend (Figure 4.6), with the v-coordinate experiencing the largest jerk.  0  0.1  0.2  0.3  0.4  0.5  0.6  0.7  0.8  0.9  1  0.9  1  CT  Figure 4.4. Velocity Profiles for a 2D Geometric Blend  _  5  L  1  0  0.1  1  0.2  1  0.3  1  0.4  1  1  0.6  0.7  I  0.5 a  Figure 4.5. Acceleration Profiles for a 2D Geometric Blend  1  0.8  Chapter 4  43  500  -500 0  0.1  0.2  0.3  0.4  0.5  0.6  07  0.8  0.9  Figure 4.6. Jerk Profiles for a 2D Geometric Blend These maximum jerks and accelerations occur at the same (T-domain location for all geometric blends. Therefore, the maximum acceleration and jerk magnitudes also occur in these fixed locations. In all cases, the profiles have the same shapes. They are simply scaled depending on the magnitude and direction of the variable changes. In other words, the location of the acceleration and jerk maxima depend on the fifth and sixth order polynomials a(d) (Eq. 4.8) and /j\d) (Eq. 4.9).  4.2.3. Limiting Acceleration and Jerk The path of a blend is not linear, thus, the acceleration and jerk vectors are not tangent to the path. Therefore, limiting the acceleration and jerks during blends is slightly more complicated than limiting them along a straight-line path. Although Lloyd and Hayward's method minimizes the average acceleration during the blend, it does not provide a method for limiting the maximum acceleration and jerk during the blend [33]. It can be shown that given a geometric blend, the maximum acceleration occurring during the blend is s  2  l + cos6  (4.20)  where p is a proportionality constant (p = 0.8) and (cf. Figure 4.3) (4.21)  Chapter 4  44  Eq. 4.20 is based on a constant acceleration assumption. Although the acceleration is not constant during the blend, it can be shown that the velocity is symmetrical about a constant acceleration line. That is, the areas above and below the constant acceleration line are equal. The maximum acceleration always occurs at the same two values of <j. <r= 0.211 and (7=0.7887 (Figure 4.5, Appendix E). The proportionality constant, p, in Eq. 4.20 is the factor used to calculate the maximum blend acceleration based on the corresponding constant acceleration (refer to Appendix E, Section E. 1). It is, therefore, possible to ensure that the blend acceleration is limited by bounding the speed at the start and the end of the blend. That is, the maximum speed at which the blend can be started and ended, SbiendMaxAcc,  subject to the maximum path acceleration, a , is max  P • <>  x  ma  blendMaxAcc  • tightness  (4.22)  1 + COS0  The maximum jerk during a blend occurs at the start and the end of the blend (Figure 4.6) and is given by (refer to Appendix E, Section E.3): 15s  3  (4.23)  -cos Itightness  J blendMax  2  Once again, the speed can be limited such that the maximum blend jerk is equal to the maximum allowable jerk, j : max  2  • imax  blendMaxJerk  • tightness  2  '0^  (4.24)  \5cos\  Thus the maximum speed with which a blend can be started (or ended) while respecting the jerk and acceleration limits is SblendMax  ttfin(shlendMaxA  =  cc> $ blendMaxJerk  ),  (4.25)  where min is a function returning the smaller of the two speeds. 4.2.4. Extension into n-dimensions The extension of the geometric blend definition and jerk and acceleration limitations presented in Sections 4.2.1 and 4.2.3 from two to n dimensions requires the manipulation of the distance tightness. In 3D, the given distance tightness defines a sphere centred at the way-point. The blend must lie within this  Chapter 4  _45  sphere. The blend is located on the plane defined by the two vectors (prPia) and (pib-pi) and must not cross the planes formed by the vectors {prPia, ii] and {pib-pi, ii}, where u = (pi-Pia)*(pib-pi)-  (4.26)  The equations provided in Sections 4.2.1 and 4.2.3 apply directly to the 3D case. in 6D, the second case of interest in this work, a "hypercube" is determined based on the given distance of approach, tightness. A hyper-cube, rather than a hyper-sphere, is used since in 6D joint space, the tightness must be individually respected for each joint. Alternatively, a Cartesian tightness sphere for the  manipulator end point could be imposed and then converted via the inverse Jacobian to a joint space ellipsoid. However, in this case the way-points are planned in joint space. Thus, the extra conversion of Cartesian limits is somewhat costly and artificial for trajectory planning. Assuming that tightness applies to each dimension, a "hypercube" centred at p  h  with edges corresponding  to ^tightnessfromthe point pi, is defined. The point of intersection of the hypercube plane closest to pi and the 6D line, X/ is labelled p . This leads to the definition of a new tightness, tighthyper, which is the la  distance betweenp, and p , and p, andp : ia  tighthyper -  2a  ^  tightness ^ max\ ( | / > i - P o | ) ,  P-Po  ,  (- ) 4  27  :  where max\p p \) r  0  is a function that returns the largest coordinate difference between pi and po.  tightness is then replaced by tighthyper in Eqs 4.12, 4.13, 4.19, 4.20-4.24. Eq. 4.27 ensures that all axes  come within the specified limit.  4.3. Summary  The blends developed by Lloyd and Hayward [33] were introduced, and a special category of blends, geometric blends, was defined so that blend acceleration and jerk could easily be limited. Trajectories formed of straight line segments connected with blends meet the C continuity requirement 2  because: (i)  the straight line segments start and end with zero acceleration (Chapter 3) and are based on a quintic time law, and,  (ii)  the blends are sixth order polynomials that start and end with zero acceleration.  Chapter 5 Connecting the Dots In Chapter 3, a method to generate a trajectory between two one-dimensional way-points while respecting the kinematic limits was described. In Chapter 4, a description of blends, which can be used to join straight line trajectories, while maintaining C continuity, was provided. In this chapter, the method used 2  to plan a smooth trajectory passing through or close to a number of n-dimensional way-points that describe the desired path is described. Manipulator way-points can be given in either joint or task space. Thus, trajectories must be designed in either space, depending on the requirements of the user. The trajectory must then be converted into the appropriate controller command signals. A manipulator's position can be described in terms of its joint angles or in terms of its end effector position and orientation relative to a fixed coordinate system. The trajectory path may also be defined in either of these spaces. If the end effector must travel along a straight line in Cartesian space, it is most effective to plan the trajectory in task space, and then convert the task space data to joint space using inverse kinematics, and then proportionally to motor pulses. For motions described in task space, the positional path is parametrized and the path length is used to determine the trajectory through a sequence of way-points. When changing direction between two straight line segments, the blends described in Chapter 4 can be used. The orientation motion is superimposed on the positional motion. The overall method developed in this work is described in Section 5.1. For motions described in joint space, two methods have been developed. The first consists of moving along a straight line in joint space between the two way-points and using blends for changes in direction (just as in task space motion). The second consists of determining the limiting joint (i.e. the slowest joint) for the motion between the two way-points. The trajectory for the limiting joint is calculated based on the method of Chapter 3. The other joints' trajectories are determined based on the limiting joint's trajectory time. These methods are further developed in Section 5.2. Finally, the method used to plan a trajectory through multiple way-points is described in Sections 5.3 and 5.4. 46  Chapter 5  _7  It should be noted that the generation of trajectory coefficients by finding quintic control points and the trajectory generation at every controller time step are two separate processes. Initially, a trajectory between N way-points is planned in such a way that it is feasible and the corresponding quintic control points and blend coefficients are calculated and stored, as described in this chapter. Once the trajectory has been planned, this information is sent to the "parser". The parser calculates the trajectory points at each controller time step based on Eq. 5.1 for straight lines and Eq. 4.7 for blends. The parser is described in Chapter 6. 5.1. Task Space Straight-Line Mode In task space, the trajectory planning is done in two steps. First the Cartesian dimensions are considered, since they can be treated vectorial ly, then the orientations are smoothly interpolated between the waypoints. 5.1.1. Cartesian Dimensions In Cartesian space, the trajectory between N way-points is composed of straight-lines segments joined together with blends. A straight line path was chosen for the Cartesian dimensions (x,y,z) as outlined in Chapter 2 (Section 2.6.1.2). As discussed in Chapter 3 (Eq. 3.28), the straight line path is parametrized in terms of time,  \\Pe-Ps\\  where p and p are respectively the initial and final position vectors of the path and q(t) is the trajectory s  e  generated using the algorithm described in Chapter 3. q(t) starts at zero and ends at \p - p \ • Limits are e  s  placed on the trajectory derivatives, q(t), q(t) and qif). These limits are respectively the Cartesian speed, acceleration and jerk limits (Appendix F, Section F.2). To generate a blend, the blend coefficients, b b , mi and m (Eqs. 4.14, 4.15) as well as the blend halfh  2  2  time, r(Eq. 4.19), must be calculated. Eq. 4.7 can then be used to generate the position along the blend at each time interval. A motion from way-point p„ to way-point p in the Cartesian domain can be composed of various v  segments depending on the speeds given to these points (Figure 5.1): If both points are stop points (points with zero speed conditions), the motion is a straight line. If only p is a stop point, the motion is composed of a straight line followed by a blend. u  Chapter 5  48  If only p is a stop point, the motion is composed of a blend followed by a straight line. v  If neither point is a stop point, the motion is composed of a blend, followed by a straight line and another blend. Thus, pointsp andp in Eq. 5.1 are not necessarily equal to way-pointsp andp . s  e  u  s = 0,s = 0 u  v  s„ = 0,s *0  v  v  Pv  Pv Pva**  p«y ^ //////  s„ * 0, s  v  s„ ^ 0, s„ * 0  =0  Pv  Pv Pvay^  Pu •  P « i * P ^ » ^  P"  h  Figure 5.1. Four Different Cases of Straight Line and Blend Motions  Therefore, a motion, in Cartesian space, between the three way-points (po,Pi,P2), where the second waypoint is not a stop point, is composed of: (i) a straight line trajectory between points p and p , (ii) a 3D 0  la  geometric blend between points pi„ and pit, as described in Chapter 4, and (iii) a straight line trajectory leavingfrompu, and either reaching p (if p is a stop point) or p 2  2  2a  (if p is not a stop point). This set of 2  motions is repeated for any number of points (Figure 5.2). It is possible that a specified way-point speed cannot be reached because a previous way-point's speed is either too high or too low. The solution to this problem will be discussed in Section 5.4.  Chapter 5  49  5.1.2. Orientation Blending was not used in the orientation domain since it is computationally intensive. This is due to the nonlinear aspect of the orientation angles, which makes matching of end conditions complex. Instead, a quintic is used to provide the trajectory for the orientation, based on an axis-angle representation of the orientation, and the rotational velocity and acceleration are set to zero at each way-point, as described below. Although the rotational motion stops at each way-point, the Cartesian motion typically does not stop at each way-point, unless very short Cartesian motions with large orientation changes are required. In this case, it is usually desirable for the positional motion to be slow or stopped. Otherwise, an overall view of the trajectory shows that the manipulator does not completely stop moving. Only the axes related to the rotational motion stop at each way-point. The axis-angle approach for orientation motion is desirable for a number of typical industrial manipulator motions, such as screwing, turning and pouring, where it is important that the end effector rotate about a single axis. The orientation coordinates given at each point can be converted to rotation matrix form with respect to the world or machine base coordinate system. The change in orientation between two way-points (e.g. p  0  and pi) can also be represented in the form of a rotation matrix:  r  r  ' 22  ' 23  (5.2)  where R Q and R i are the rotation matrices corresponding to the orientations, expressed in the world W  w  frame, at p and pi respectively. Every rotation matrix can be expressed as a rotation of a given angle 0  about an arbitrary axis in space [42]. Thus, to move smoothly between p and pi in the orientation 0  domain, the orientation trajectory must rotate about the axis of the rotation matrix, R \. The angular 0  rotation and acceleration about that axis must be continuous in order to ensure limited jerk in orientation space.  Chapter 5  50  The orientation trajectory is based upon a stop point to stop point quintic (Figure 5.3). That is, a quintic with zero velocity and acceleration end conditions. The quintic starts at position zero and ends at position  r  <f) - COS'II 2  + r  —1  + r 22  33  1  (5.3)  where rn, r , r are obtained from Eq. 5.2. The time allowed for the quintic will be discussed in the next 22  3i  section. Setting the start and end rotational velocities and acceleration to zero does not provide the most timeoptimal solution. However, it makes the superimposition of the orientation and positional trajectory components tractable for on-line computation.  time [s]  Figure 5.3. Rotational Motion Trajectory It is possible to limit the angular velocity, acceleration and jerk about the axis, by allowing enough time for the motion based on: (5.4)  857.. 10<Z>  2  (5.5)  Chapter 5  51  and 60«^ ] 3  (5.6)  2  where to, is the minimum motion time necessary so as not to violate the rotational velocity limit, ca„ , tx is ax  the minimum motion time necessary so as not to violate the rotational acceleration limit, X„ , and t is the mx  y  minimum motion time necessary so as not to violate the rotational jerk limit, (p , when rotating about an max  axis by fo radians with zero rotational velocity and acceleration end conditions. 5.1.3. Combining Cartesian and Orientation Trajectories and Generating a Trajectory A task space motion is formed of an orientation motion superimposed upon a positional motion. In order to superimpose these two motions, they must have equal trajectory times. The Cartesian motion time is composed of various components since it can be formed in four different ways as described in Section 5.1.1. A blend around a point, p , is divided into two, so that the first half is w  allotted to the motion between points p .i and p and the second half to the motion between points p and w  w  w  _?„.+/. Thus, the Cartesian motion time between two way-points, p and p , has three components (some of u  v  which may be null): the first half-blend time, T , the straight line motion time, t i, and the second halfu  s  blend motion time, r„. The total positional time, carl  (5.7)  — T„+ t / + T s  v  is the time necessary to move between way-points p and p in the Cartesian domain while respecting all u  v  Cartesian limits. The orientation motion time is more flexible, provided that the rotational limits are respected. Therefore, once the optimal Cartesian motion time has been determined, it is compared to  (Eq. 5.4), tx (Eq. 5.5)  and t (Eq. 5.6). If v  t  >t ,, curl  where t  ro1  = max\$ ,\' xd ), m  f  then t  .  roi '  (5.8) v  '  car  , is the time for the motion between p and p . One can note that there u  v  is an added advantage to using an orientation quintic with null speed and acceleration end conditions: its time can be easily modified without causing adverse effects, as long as Eq. 5.8 is satisfied (retrograde orientation motion is a low risk issue).  Chapter 5  52  If Eq. 5.8 is not satisfied, then the motion cannot be carried out in time t  ,  car  without violating the rotational  limits. In this case, the second way-point, p is redefined as a stop point, and t v  rot  is assigned as the motion  time. The Cartesian trajectory is then recalculated. If /,.„, is larger than the time necessary to move from p  u  and p with p as a stop point, then the positional components are held constant until the rotational motion v  v  has ended. This does not provide the fastest possible motion between the two points (since it might be possible to end the motion with a smaller than specified Cartesian velocity); however, it is a computationally efficient solution.  5.2. Joint Space Mode 5.2.1. Straight Line Joint Space Motion Planning in joint space is done in a similar manner to planning the task space Cartesian motion. That is, the trajectory is planned along straight-line paths in n-dimensional joint space connected with blends (where n is the number of actuators). One can note that a straight line in joint space does not imply a straight-line task-space motion of the manipulator end effector. A straight-line joint-space path is used because it is the shortest path between the two joint space points. Furthermore, Sahar and Hollerbach show that the time-optimal motion (considering dynamic limits) between any two points approaches a straight line in joint space [41]. Finally, a straight line path also allows for coordinated axis motion. All axes reach the way-points at the same time, starting and stopping in a coordinated manner, and the motion path is independent of the speed along the path.  The straight line path is parametrized using Eq. 5.1. Limits are placed on the trajectory derivatives, qit), q{t) and q{t), thus, the overall path limits are used rather than individual joint limits (refer to Appendix F, Section F.l). In joint space, a motion between three way-points, (po,Pi,P2), (where the second way-point is not a stop point) is formed by a straight line trajectory between points p and p , a 6D blend between points p 0  ia  !a  and  p, . as described in Chapter 4, followed by a straight line trajectory leaving fromp and either reaching p b  lb  (if p is a stop point) or p 2  2a  2  (if p is not a stop point). Again, this motion can be extended for any number 2  of points (Figure 5.2), and the same four path possibilities exist in joint space as in task space (Section 5.1.1).  It is possible that a specified way-point speed cannot be reached because of a previous point's  speed being either too high or too low. The solution to this problem will be discussed in Section 5.4.  Chapter 5  53  5.2.2. Independent Joint Motion A further motion option was examined for joint space planning: that of planning the joint trajectories independently. That is, the limiting joint for a motion between two way-points is determined and the optimal motion for this joint planned using the method of Chapter 3. The other joint motions are then determined based on this time and the demanded way-point speeds using a series of concatenated quintics generated such that the individual joint jerk, acceleration and speed limitations are respected. The waypoint speeds are attained, if feasible. A motion between two joint space way-points may require some joints to move a large distance and others to move a smaller distance. Thus, it may be necessary to stop the joint travelling a short distance during the motion and then accelerate it to reach the demanded end speed when the slower joint is reaching the end of its trajectory. This leads to uncoordinated joint motion: that is all joints may not start and stop at the same time. In addition, this type of motion may produce varying trajectory paths as the way-point speeds are increased or decreased. For these reasons, it was determined that independent motion was not desirable. However, it was found that this type of motion is faster than straight line joint motion for a multiple way-point path because the way-point speeds can be specified and reached independently for each joint. 5.3. Planning a Straight-Line Motion Between Two Way-Points With Variable Speeds In Chapters 3, 4, and the preceding sections all the components necessary to plan a trajectory between way-points with non-zero speeds have been established. This section provides an overview of how a trajectory between any two given way-points defined in either n-dimensional or task space is planned (Figures 5.4-5.6). Assuming a motion between two way-points, />,, and /?,+/ is being planned, it is first necessary to check whether the acceleration and jerk limits will be violated during the blend around point p,+i (if p i is not a i+  stop point). If the limits will be violated, the speed at p  i+I  is reduced using Eq. 4.25. Then, the start and  end points of the straight-line segment, p and p , are determined based on the blend requirements at each s  e  way-point. At this point, the planning method differs depending on whether the planning is being carried out in task space or joint space. In the case of task space planning, the quintic control points corresponding to the trajectory along the straight line segment in Cartesian space are determined, along with the blend coefficients for a blend  Chapter 5  54  around p,- (Figure 5.5). The orientation parameter, <p , (Eq. 5.3) is then calculated, and the method +/  2  described in Section 5.1.3 is used to generate the overall trajectory. Joint space or n-dimensional planning is much simpler: the quintic control points corresponding to the straight line segment are calculated, as are the blend coefficients for the blend around />,•+/, if necessary (Figure 5.6).  modify s,+/ (Eq. 4.25) X  Chapter 5  55  Calculate the quintic reference points for a trajectory from p to p in the Cartesian space (Chapter 3). s  e  Calculate the blend coefficients and time for a blend from pp+na to P(i+i)b (Eqs. 4.14, 4.15,4.19).  Calculate angle necessary to rotate between orientations p and p,+i. (Eq. 5.3) t  Calculate the quintic reference points for a trajectory from p to /?,-+/ in the Cartesian space using method of Chapter 3. Most of the motion involves a change of orientation with a little change in Cartesian position. s  quintic control points, orientation angle, blend parameters  Figure 5.5. Cartesian Space Trajectory Reference Data Generation  Chapter 5  56  t Calculate the quintic reference points for a trajectory fromp to p (Chapter 3). s  e  Calculate the blend coefficients and time for a blend from pa+i) to pp+nb (Eqs. 4.14, 4.15, 4.19). a  quintic control points, blend parameters  Figure 5.6. Joint Space Trajectory Reference Data Generation 5.4. Backward Planning Issues and Trajectory Generation Between N Way-Points When way-points are specified, they are assigned a demanded speed that should be reached by the time trajectory arrives at the way-points, if possible. For example, a task space way-point can be specified (using roll, pitch, yaw angles) as p = (x, y, z, y/ , y/ , y/ , s%), where s% varies between 0 and 100, 0 x  y  z  meaning p is a stop point, 100 meaning that the machine should pass through the point at the manufacturer maximum allowable speed (MMAS) - often a "worst case" kinematic limit. Given three way-points, p , pi, and p , with demanded speeds speed = 0, speed) = s/, and speed = 0, 0  2  0  2  through which a trajectory must be fitted while respecting the kinematic limits, there are three possible scenarios (Figure 5.7): (a) The demanded speed, s at pi is reached. h  (b) The demanded speed, s ,dXpi is not reached because of a distance limitation between points p and t  0  Pi  (c) The demanded speed, s , at p is not reached because of a distance limitation between points pi andp . t  t  2  Chapter 5  57  •  actual speed profile demanded speed speed attained at point 1  attainable speed profile limiting distance  (a)  0.2  0.4  0.6 time [s]  0.8  1  1.2  Figure 5.7. Three Possible Speed Profiles Between Three Way-Points. The trajectory planning between the three points (po,pi,P2), N=3, is carried out as follows (Figures 5.8, 5.9): 1.  Plan a trajectory between points p andp,. 0  IF the demanded speed at point p cannot be reached (case (b)), t  Replace the demanded s with the reachable s/. t  2.  Plan a trajectory between pointsp andp . t  2  IF the demanded speed at point pi cannot be reached (since the zero speed condition at p must be respected), 2  Chapter 5  58  Replace the demanded s with the reachable 5/, and replan the trajectory between p and p t  0  h  with this new  end speed. This approach is referred to as backplanning.  This trajectory planning method can be extended to any number of way-points. The planning is carried out from the first point to the last point, with backplanning done as necessary. Figure 5.8 and 5.9 show a flowchart of the trajectory planning method between N way-points. The calculated reference data (namely, the quintic control points and the blend parameters) are then used to generate trajectory points at every controller sampling interval (Chapter 6). It should be noted that by planning the trajectory based solely on the reference data, it is computationally simple to modify the trajectory when speed corrections are necessary. That is, only the control points and blend parameters must be modified.  5.5. Summary  In this chapter, a method for designing a smooth near time-optimal trajectory that passes through or close to many way-points, while respecting speed, acceleration and jerk limits, was described. The method is based on straight line motion in any n-dimensional space and the use of geometric blends for changing direction. The trajectory generation method described in Chapter 3 is used to generate the time law associated with the straight line paths. The geometric blends described in Chapter 4 are used to interconnect the straight-line paths. Planning the trajectories is computationally simple. It requires only the computation of a series of reference data for each way-point to way-point motion: the quintic control points (a maximum of eight per way-point to way-point trajectory) and the blend parameters.  Chapter 5  Figure 5.8. Trajectory Planning Between N Way-Points (Forward Planning)  59  Chapter 5  60  i from forward planning (Fig. 5.8)  replace s, with attainable s,  j= i-l  Plan trajectory from/?, to/>,+/. (Fig. 5.4)  Replace s,.  j = j-l  Back to forward planning (Fig. 5.8)  Figure 5.9. Backplanning  Chapter 6 Application to a Manipulator In the overall trajectory planning algorithm there are three different cooperating systems (Figure 6.1): a low level, which calculates the quintic control points and blend coefficients (Chapter 5), a parser, which calculates the trajectory points corresponding to each sampling interval in task or joint space, and a high level, which coordinates the flow of information from the scheduler to the low level and then to the parser. The high level and parser functions discussed in this chapter are intended for an on-line manipulator system. high level components  Way-point position, speed, tightness Joint space or Cartesian motion Stop point? W ;V way-poml-. al ii lime  Check (vol) way-points.  Low level  Set flags.  quintic control points blend parameters Parser Figure 6.1. Interaction Between the Three Planning Units 61  Chapter 6  62  6.1. Way-Points The way-points are provided by a high level scheduler. This may be a scheduler program with an off-line set of pre-selected way-points, or an on-line workcell scheduling program. These way-points designate the manipulator path. Along with each way-point position, specified either in joint space or in task space, the scheduler must also indicate the speed the robot should reach at the way-point. This speed can be specified as a percentage of the maximum speed that the robot can attain, as determined by the manipulator manufacturer. The scheduler must also specify other properties related to the way-point, namely: the tightness associated with the way-point, i.e. how closely the way-point should be approached (cf. Chapter 4); whether the way-point is a stop point; and those related to the motion that starts at the given way-point: whether the motion should be done in joint space or cartesian space; whether the wrist orientation should be maintained or allowed to vary. This property can only be specified in the case of a task space motion. It is important in the case where the robot must not tip its payload. 6.2. High Level Planner When the high level planner receives way-points from the scheduler, it places them in a queue. The planner processes a maximum window of N way-points at a time. At the beginning of a planning sequence, the first Af way-points are checked for scheduler inconsistencies, and a number of variables necessary for the low level trajectory planning are set. This process is referred to as vetting the waypoints. Once vetting has been performed, the way-points are sent to the low level trajectory planning algorithm, and the trajectories between the entire window of N way-points are planned. The quintic control points and blend parameters associated with a certain number of those N way-points are then sent to the parser. The high level selects the succeeding way-points in the queue and the cycle recommences. This continues until no new way-points are available in the queue. This feature allows the trajectory to be computed on-line, since the time needed to compute the trajectory can be fixed by carefully selecting the window size, N. The following subsections explain the overall process in more detail. 6.2.1. Preliminary Considerations Way-points can be delivered to the queue asynchronously. Therefore, the high level planner cannot predict the number of way-points it is likely to receive. When planning the trajectory between the current  Chapter 6  63  window of N way-points, a zero speed condition is imposed by the algorithm on the A " way-point to 7 1  ensure that the manipulator will stop at this "last" way-point, in the event that no further way-points are delivered. In addition, if the manipulator completes the previously planned trajectory before the trajectory for the current window of N way-points is planned, the manipulator will be able to halt and wait for the computations to catch up. This case could occur when the N way-points are very close together, or when the number N is rather small. Thus, it is necessary to ensure that the planning window size, N, is large enough to avoid computation stalling. The algorithm-imposed zero speed condition is removed later in the planning, if possible, as described in Section 6.2.3. A point with a zero speed condition is called a stop point. As mentioned above, the planning window size, N, should be chosen such that the planning can be done on-line. However, N must also be chosen in such a manner that it is possible to reach a significant speed when travelling between the N way-points (e.g. if N = 1, then each point would be a stop point, which would obviously lead to a slow trajectory). 6.2.2. Vetting The vetting function is provided to check the scheduler way-point information. In the vetting function (Figure 6.2), the way-points are compared to verify that two consecutive way-points are not located "on top" of each other. In this case, the second of the two way-points is removed. Furthermore, if a way-point lies within the next way-point's tightness (encompassed point case, Figure 6.3), the second of the two way-points is eliminated. In reaching the first way-point, the second way-point's tightness area will be entered and, therefore, the second point can be considered to have been reached. If the tightness of two way-points overlap (overlapping tightness case, Figure 6.3), they are reduced proportionally such that they become tangent to each other. This is done so that it is possible to plan the two blends associated with the points. In the example provided in Figure 6.3, way-point p is eliminated since p, lies withinp 's 2  2  tightness. The tightness ofp and that of p are reduced proportionally such that they are tangent to each 5  6  other. In some unusual encompassed point cases, it is possible that the resulting path will not enter an eliminated way-point's tightness area (Figure 6.4(a)). If such cases are of concern, the point should be treated as an overlapping tightness case, although this will result in a slower motion (Figure 6.4(b)). Once all point elimination has been performed, the last of the N way-points is set to be a stop point (algorithmimposed zero speed condition) as explained in Section 6.2.1.  The vetting function also sets the variables related to the motion between two consecutive way-points. The motion information associated with the trajectory between two way-points is always assigned to the  Chapter 6  64  first of the two way-points. The trajectory motion type is set to be either a straight line followed by a blend or a straight line followed by a stop point. This variable is used to determine which low level function is used to compute the trajectory quintic control points and blend parameters. As well, the maximum path speed between two way-points is set to be the maximum of the speeds associated with the way-point under consideration and the subsequent way-point.  N way-points  i =  0  Eliminate way-point p,i-H-  Pi within tightness,^  Eliminate way-point p,  tightness, intersects tightness\+\l  Reduce tightness, and tightness,+/ so that they are tangent.  Set motion type.  •w(0 = maxiX s ) m  i =  i+l  s= 0 m  To low level Figure 6.2. Vetting Procedure  Chapter 6  65 encompassed point overlapping tightness — ——  /' Pf \ \  \  P  !  2  /  /  ) ^'  v  1  •  \ S  ^y  i  [PI.  „  ^  /  !  V--'  \  J  \  «p.  v  ^  y  Figure 6.3. Elimination of Way-Points Due to Tightness Considerations /  y  y  \  < \ \ \  \  \ \ 1  P^ /  \  /  /  '—-—  y  V.  -  y  —  \  \  V— \sr y N  /  PI *  y  //  :  ' P£-~~\— (a) Illustration of a case where the elimination of point p would result in the path not entering the tightness area of point p - encompassed point case. 2  2  /  y  y"  N \  \  i P3^__ •  i  V 1  —  \  /  \  /  y'  >.  ~  /  V  '  /  •  \  P&A—"  (b) Suggested Solution: Reduce the tightness of way-points 1 and 2 proportionally - treat p, and p as an overlapping tightness case. 2  Figure 6.4. Special Cases for Point Elimination in Vetting Algorithm  Chapter 6  66  6.2.3. Trajectory Planning and Parser Points Once the vetting has been performed, the trajectories are planned sequentially between each pair of waypoints, as described in Chapter 5. It is then necessary to send the quintic control points and blend parameters to the parser. However, as mentioned in Section 6.2.1, a zero speed condition is imposed by the algorithm upon the last of the N way-points. Obviously keeping this restriction is not optimal, and it should be removed, if possible. This possibility was incorporated through the use of flags. The flags are also used to determine the number of planned trajectories that will be sent to the parser. There are two flags: Desirable, and Best. A way-point is Desirable when the trajectory passes through , this way-point at the speed requested by the scheduler. A way-point is Best when it is Desirable or when its speed is imposed by the previous way-point (i.e., the speed achieved is the highest possible given the circumstances). The Best flag indicates the latest way-point in the planning window that will not be affected by the addition of new way-points to the queue, or by the removal of the zero speed condition on the A " way-point. Therefore, a way-point with an algorithm-imposed zero speed is never Best. 7 1  A way-point's flags are set once a trajectory between a pair of way-points has been calculated, and the corresponding back-planning performed. If the planning between the two way-points currently under consideration required backplanning of the previous way-points (Section 5.4), the Best and Desirable flags of the backplanned way-points are modified during the backplanning. An example is provided below, in Section 6.2.3.1. Once planning in the window is completed, all W way-points preceding the last Best way-point in the window are passed to the parser since there is no way to improve the trajectories linking these way-points. If new points are available, the zero speed condition is removed from the last point in the window, and the next available W way-points in the queue are added to the window. This new list of way-points is processed by the high level. This approach provides an optimally planned trajectory since there are no algorithm-imposed stop points. In the event that the time it will take the manipulator to reach the last of the W way-points is smaller than the time it will take the planner to process the next window of N way-points, it is necessary to provide Y (Y > W)  way-points to the parser. In this case, the (F+l)" way-point is marked as Best and its speed 1  cannot be modified. Then Y new way-points are accepted from the queue. This obviously does not provide the most optimal trajectory, but it allows for continued on-line computation of the trajectory.  Chapter 6  _7  6.2.3.1. Flag Setting Example In this illustrative numerical example, there are eight way-points in the window (N = 8) with desired speeds given in Table 6.1. The way-points are designated as pi,p , —,ps, their corresponding speeds and 2  flags as, respectively, s/, Best/, Desirable;, etc. The speeds are given as a percent of the manufacturer speed limit. The planner algorithm proceeds as follows: 1.  T h e speed at p  2.  B e s i \ is set to T R U E .  3.  Forward plan from p to p : s = 40% is reached, therefore, Desirabtei = T R U E .  4.  Forward plan from p  7  Desirable  2  5.  is set to zero (zero speed condition).  8  2  2  to p : s = 55%. Best  2  3  3  2  = T R U E (p 's 2  speed was imposed by p?,  and  3  = TRUE.  Forward plan from p  to p .  3  s  4  reduced to 25% to achieve s  3  4  = 20%, due to distance limitations.  Therefore, B e s f = F A L S E (p 's speed is imposed by p ) and Desirable 3  a.  not p )  3  Backplan: from p  2  4  s  to p : 3  2  = FALSE.  3  reduced to 35% to allow s  3  = 25%. Best  = F A L S E , Desirable  2  =  2  FALSE. b.  Backplan: from p to p to allow s = 35%. No change in flags for p . 1  2  2  7  6.  Forward plan from p to p : s = 30%. Therefore, Best = T R U E and Desirable  7.  Forward plan from p  5  to p : s = 45%. Therefore, B e s f = T R U E and Desirable  5  = TRUE.  8.  Forward plan from p  6  to p : s = 55%. Therefore, Best = T R U E and Desirable  6  = FALSE.  9.  Forward plan from p  4  5  5  4  6  6  5  7  7  e  7  8  8  7  8  Backplan: from p  = TRUE.  to p : s reduced to 40% to allow s = 0. Therefore, S e s f = F A L S E (p 's  7  is imposed by p ) and Desirable a.  4  6  7  speed  7  = FALSE.  to p : s = 45%. No change in flags. 7  6  The final values of the way-point flags are shown in Table 6.1. The information related to the trajectories linking pi to /?6 can be sent to the parser since even if the zero speed limitation is removed from p , these 8  trajectories will not be affected. Point p will become the new first way-point in the window of N way6  points, and five new way-points will be accepted {W = 5). When the new list is compiled, s will be reset 8  to the scheduler's demanded speed, and the trajectory segments between p and pg will be recalculated. 6  way-point  1  2  3  4  5  6  7  8  demanded speed (%)  10  40  60  20  30  50  60  50  speed attained (%)  10  35'  25  20  30  45  40  0  Best  TRUE  FALSE  FALSE  TRUE  TRUE  TRUE  FALSE  -  Desirable  TRUE  FALSE  FALSE  TRUE  TRUE  FALSE  FALSE  -  Table 6.1. Way-Point Speeds and Flags at the End of Planning  Chapter 6  68  6.3. Parser The function of the parser is to compute the n-dimensional reference signal corresponding to each sampling interval. The parser works in parallel with the high level and low level planners. That is, the parser computes trajectory points for each sampling interval from the previously planned trajectories, while the high and low level planners are generating new quintic control points and blend coefficients to be fed to the parser. As mentioned in Section 5.1.1, a motion between two way-points, p and p , can be composed of various u  v  segments depending on the speeds of these two way-points: If both way-points are stop points, the motion is a straight line. If only p is a stop point, the motion is composed of a straight line followed by a blend. u  If only p is a stop point, the motion is composed of a blend followed by a straight line. v  If neither way-point is a stop point, the motion is composed of a blend, followed by a straight line and another blend. Therefore, there are two different types of parsing corresponding to the components of the motions being generated: straight-line parsing and blend parsing. The parser recognizes which segments are present in the motion between two way-points and generates the appropriate controller commands based on the time data provided by the high level (i.e. a segment with zero time is not present in the motion). The routines outlined in the next three sections are used to calculate the n-dimensional reference trajectory corresponding to the sampling intervals. 6.3.1. Straight Line Parsing For a straight line, it is first necessary to determine the quintic coefficients, b , s  b (Eq. 6.1) based on 0  the quintic control points provided by the planner. By taking the first and second derivatives of Eq. 6.1 with respect to time, six equations, corresponding to the start and end points of the straight line, can be derived. g(t) = b/ +b/  +bf  +bf  +b t + b , l  l)  (6.1)  where t is the time, the path parameter. Since the position, speed and acceleration are known at the start and the end of each quintic, the system of six equations and six unknowns (the quintic coefficients) can be solved. This system is shown in Eq 6.2:  Chapter 6  69  1  t? 1  2/,  0  2  0 0 1  h  0  1  0  0  t! It?  Pi  t\ ~V st: \2tf 20t b tl b 5t K \2t\ 20tl_b <,  4  "i  3  t  3  tl 2t 2  '2  2  3/  2  2  «i  3  Pi  4  v  2  6*  2  (6.2)  2  5  - 2_ Q  where t pi, v ai are the time, position, speed and acceleration at the start of the quintic, similarly, t ,p , h  h  2  v , a are the time, position, speed and acceleration at the end of the quintic, and b , 2  2  5  2  b are the unknown 0  quintic coefficients. To simplify this system, the initial time parameter, <*, is set to zero for each quintic trajectory segment. The top of the matrix becomes diagonal and the system is reduced to a system of three equations and three unknowns, which is suitable for solving on a DSP-based trajectory planner. There are no transcendental calculations involved in the calculation of the quintic control points needed to describe the trajectory between two way-points. The limiting case, computationally, is that described in Section 3.2, case B, where seven quintics may be necessary. In this case, a maximum of 480 floating point operations is required to determine the quintic control points. Once the quintic coefficients have been determined, it is possible to calculate the n-dimensional reference signal corresponding to each sampling interval by simply incrementing the time in Eq. 6.3. p(0=P. +  j^4*(0.  (6-3)  \\P -Ps\\ e  where  t =k-At, 0<k<^-, At k  (6.4)  p and p are respectively the initial and final position vectors of the straight line and At is the sampling s  e  interval. g(t) starts at zero and ends at \p ~ p \ • e  s  6.3.2. Task Space Parsing When the trajectory is defined in task space, the Cartesian portion of the trajectory is generated based on Eq. 6.3. The orientation portion of the trajectory is generated using a different method because the orientation axes must be orthogonal (i.e. orientation is not a vector). Eq. 6.1 is used to generate a quintic that rotates the orientation about a fixed axis in space, as explained in Section 5.1.2. Since the quintic provides an angle corresponding to each sampling instant, it is necessary to convert this axis-angle representation to the original orientation specification at each increment. This is done using quaternion representation. That is, the relative axis-angle representation is converted to quaternion form. The  Chapter 6  70  necessary matrix manipulations are done in quaternion representation to obtain the orientation in terms of the world reference frame, and then converted to rotation matrix form, providing a form that can easily be mapped to the type of coordinates being used by the controller. This algorithm detailed in Appendix G. 6.3.3. Blend Parsing To generate the n-dimensional trajectory corresponding to a blend, the blend parameters, bi, b , nti, m 2  2  and x, calculated during the low-level planning (Section 4.2.1), are used in Eq. 6.5: x{a )=x {a ) + a{a \x (o ) - x, [a )) - ic/3{a )v , k  {  k  k  2  k  k  where  k  d  (6.5) (6.6)  t = k-At,0<k<2T,  (6.7)  k  • * i f o ) = *i + "*i  (6.8)  k>  ff  (6.9)  x {^ )=b +m (T , 2  k  2  2  k  a{cj )=6c7 -\5a  A  £(ff*)=<r* ~  5  5  k  k  k  6  3cr  +\0a  3 k  * + "/ 30  ,  (6.10)  -°k\  (6.11) (6.12)  and  K  = 15/2.  (6.13)  6.4. Summary In this chapter, the issues related to the implementation of the trajectory generation method described in Chapter 5 in a manipulator system were discussed. Such an implementation requires a high level that controls the flow of information between the scheduler and the planning algorithm, as well as to the parser. The parser is the unit that computes the reference signal corresponding to each sampling interval. A flag is used to determine which points to passfromthe planning algorithm to the parser with the aim of providing the fastest trajectory possible between the entire queue of way-points given the imposed kinematic limits and the time required to plan the window of way-points.  Chapter 7 Experimental Results In this chapter, the quintic-based trajectory generation algorithm is compared to an industrial trajectory generator [6], and to the smooth, path-constrained time-optimal motion (SPCTOM) technique [14]. The first comparison highlights the improvements in the trajectories as compared to the industrial technique. These comparative approaches provide a good comparison metric and were readily available for testing. 7.1. Experimental Setup The experimental setup used to test the trajectories is a CRS A465 6-dof manipulator. The manipulator is controlled with a C500C CRS controller. Two different trajectory planning algorithms were available for use with the C500C controller. The first is the algorithm previously used by CRS and has no jerk limitations. It is referred to hereafter as the industrial planner. The second is an industrial implementation on the C500C of the algorithms developed in this thesis and licensed by the University of British Columbia to CRS Robotics Corp. Herein, it is referred to as the quintic planner. The experiments compare the industrial and quintic trajectory planners over a number of different metrics. The encoder feedback is read from a debugging header in the C500C controller using an open architecture real-time operating system (ORTS) [21]. In addition, the trajectory generation method proposed herein is compared to the SPCTOM trajectory using a SCORBOT ER-Vfl 5-dof manipulator in the IAL at UBC [14]. For this experiment, the robot is controlled by a TMS320C32 DSP board interfaced with two MFIO-3B cards that each control three axes. ORTS is used to send the control signals to the robot actuators based on the pre-planned trajectories. A tuned discrete PID controller is used to control the manipulator. In this case, only the positional degrees of the manipulator were controlled. The Denavit-Hartenberg (DH) parameters for the SCORBOT ER-VII and the A465 are provided in Tables 7.1 and 7.2 respectively. The experimental setup is shown in Figure 7.1. Link  6 [radl  d [mm]  a [mm]  1  0  358.5  50.0  2  a [rad] n  ~ ~1  0 -37.0 300.0 0 3 0 0.0 250.0 0 Table 7.1. Denavit-Hartenberg Parameters for the Positional dof of the SCORBOT ER-VII 71  Chapter 7  72  Link  9[rad]  d [nun]  a [mm]  1  0  330.2  0.0  2  0  0.0  304.8  3  0  0.0  0.0  4  0  330.2  0.0  5  0  0.0  0.0  6  0  a [rad] 71 ~~2  0 71 2  71 ~~2  71 2  76.2 + tool 0.0 0 transform Table 7.2. Denavit-Hartenberg Parameters for the CRS A465 Manipulator  Figure 7.1. Experimental Setup (CRS A465 manipulator in the foreground; SCORBOT ER-VTI manipulator in the background)  Chapter 7  73  7.2. Comparison to a Typical Industrial Trajectory Generation Method In this section, comparisons between the quintic planner and the industrial planner are made. Four exemplar types of motion are presented in this section, namely: (1) task-space stop point to stop point motion, (2) task-space multiple way-point motion, (3) joint-space stop point to stop point motion, (4) joint-space multiple way-point motion. The kinematic limits of the CRS A465 manipulator are given in Tables 7.3 and 7.4. Task space rotational jerk limitations were not included in the CRS implementation of the quintic planner. As mentioned, jerk limitations were not be implemented on the industrial planner. The motions were run on the CRS A465 manipulator, five times each, at 10 different "demanded" path speeds for a total of 50 data sets per motion for each planner. The CRS system requires the demanded motion speed to be input as a percentage of the manufacturer maximum allowable speed (MMAS). The first five trials for each motion were carried out with a demanded speed of 10% of the MMAS. The demanded path speed was then incremented by 10%, up to the MMAS, and five trials were run at each increment. Since the same control laws (PID) are used for the industrial and quintic planner, any changes in path tracking or motion time for similar motions are due to the nature of the trajectories generated by the planner. The experimental data provided in this section are averages reported to the appropriate accuracy based on two standard deviations of the data. Only the actuator encoder feedback was available for analysis. This precluded examination of the jerk profiles, since the jerk was indistinguishable from the noise of the signal. In addition, the motion times reported are the implemented motion times. That is, once the encoder speed reached a certain level (distinguishable from the noise of the signal), the motion was deemed to have commenced. The motion was deemed to have ended when the speed dropped below the signal noise level. Thus, the actual motion times can be compared between planners, but they are not identical to the motion times provided by the reference signals. The results provided in this section are a selection of the results available. The complete comparative results can be found in Appendix H, including an analysis of the relative improvements achieved using the quintic planner approach. Joint 1  Joint 2  Joint 3  Joint 4  Joint 5  Joint 6  Speed (MMAS) [rad/s]  71  K  IX  96;r 101  96n 100  96x 101  Acceleration (MMAA) [rad /s ]  4TC  471  47C  800;r 101  800;r 100  800;r 101  320TC  64000/r 101  64000;r 100  64000;r 101  2  Jerk 32071 320TC [rad Is ] Table 7.3. Joint Kinematic Limits for the A465 3  Chapter 7  7  4  Cartesian Rotational Speed (MMAS) 1016 mm/s 2 rad/s Acceleration 2540 mm/s 10 rad/s Jerk 81280 mm/s Table 7.4. Task Space Kinematic Limits for the A465 2  2  3  7.2.1. Task-Space Stop Point to Stop Point Motion In the straight-line task-space motions considered here, it is desired that the end effector accurately track the specified path. Therefore, the deviation of the end effector from the straight line is of importance, as well as the motion time. Since it is desired that the motions being compared in task space result in relatively similar joint paths to provide a proper basis for comparison, two types of experiments were carried out: motions that comprised purely positional changes and a motion that provided a change in task space orientation but no change in end effector position. To allow consistent comparison, it is necessary to separate these two types of motions since the industrial and quintic planner plan the orientation trajectories differently. In the positional task-space experiments the motion times and the path deviations (shortest distance from each data point to the desired straight-line path) along the trajectory are compared, along with the maximum occurring path deviation. In the orientation experiment, the motion times, as well as the smoothness of the motions, are compared. 7.2.1.1. Experiment 1: Task-Space Straight Line with Constant Orientation  Figure 7.2 shows a comparison of the industrial and quintic task-space trajectories, as tracked by the CRS controller, along the straight-line path between the two points given in Table 7.5. Both planners generated a straight line reference signal. However, because of the way that the quintic task space trajectory is shaped, the controller is able to more accurately track the desired path at all demanded speeds. Table 7.6 gives a summary comparison of the deviations from the straight line path for both planners at various demanded speeds (refer to Section H. 1.1, Appendix H for the complete results). In this example, the average and maximum path deviations obtained using the quintic planner with the demanded path speed equal to the MMAS (1016 mm/s) are better than those provided by tracking of the industrial planner trajectory at a demanded speed of 102 mm/s (10% MMAS). Both planners exhibit an increase in average and maximum path deviation as the maximum allowable path speed is increased. The quintic planner generates a faster trajectory when the demanded path speed is below 406 mm/s (40%o MMAS). The industrial planner accelerations do not reach the manufacturer maximum allowable  Chapter 7  75  Chapter 7  76  acceleration limit ( M M A A ) at these speeds. As the demanded path speeds increase, the industrial planer increases its maximum path acceleration until it eventually violates the M M A A by a significant margin (cf. Figures 7.3(a) and 7.5(b)). One can note that at 100% maximum speed, the industrial planner demands too high an acceleration and the resulting speed profile exhibits oscillations (Figure 7.3(a)). The quintic planner, on the other hand, requires a consistent maximum acceleration level for all desired path speeds, and the acceleration profile generated does not significantly violate the M M A A even at the maximum speed (Figures 7.3(b) and 7.5 (b)).  Point  x [mm]  y [mm]  z [mm]  yaw [rad]  pitch [rad]  roll [rad]  1  510  355  310  0.165  0.0731  -0.0113  2  555  -360  240  0.165  0.0731  -0.0113  Table 7.5. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 1 (tool transform of 10.5 mm in z-direction) 102 (10%)  Demanded speed:  305 (30%)  711 (70%)  350  350  300  300  250  __250  :200  I 200  I 150(1  $150  100H  100  Q. m  1016 mm/s (100%) (of M M A S )  (/> Q.  50 50  0  I  (  4  0o  time [s]  1200  1200  1000  1000  800  ' 800  600  •o 600 co CU  w 400  "400  200  200  0.  4  time [s]  0.5  time [s]  1  (a) Industrial Planner  1.5  0. "0  0.5  time [s]  1  (b) Quintic Planner  Figure 7.3. Comparison of Speed During Motion of Task Space Experiment 1  1.5  Chapter 7  77  In Figure 7.4, the path deviation plots for two different demanded path speeds are shown with respect to time. Comparison of the plots in Figure 7.4 to the acceleration profiles of Figure 7.5 demonstrates the role of the acceleration and jerk limits in path tracking. An increase in path deviation is experienced at the beginning and end of each motion due to the non-zero acceleration components, which increase the dynamic load on the system. The path deviations are larger when the industrial planner is used. A comparison of Figures 7.4(a) and 7.5(a) shows that these larger path deviations are related to the jerk profiles since, in this case (at 10% MMAS), the industrial planner maximum acceleration and deceleration are far below those generated by the quintic planner.  Industrial Planner Quintic Planner  Industrial Planner! -Quintic Planner  E1.5 E, c o  ra 1 '>  C D TJ  1 0.5  0 0  2  4 time [s]  6  8  0  0.5  ,, time [s]  1  1.5  (a) Demanded Speed: 102 mm/s (10% MMAS) (b) Demanded Speed: 1016 mm/s (100% MMAS) Figure 7.4. Comparison of Path Deviations of Task Space Experiment 1  x 10 1.5  2000  Industrial Planner -Quintic Planner  Industrial Planner -Quintic Planner  1000 1  E  -1000  -2000 0  -0.5  2  4 time [s]  6  8  -1 0  0.5 .. . , time [s]  1  1.5  (a) Demanded Speed: 102 mm/s (10% MMAS) (b) Demanded Speed: 1016 mm/s (100% MMAS) Figure 7.5. Acceleration Profiles (filtered) of Task Space Experiment 1 (Note different acceleration scales)  Chapter 7  Demanded Speed Along Path [mm/s (MMAS)]  78  102 (10%)  305 (30%)  711 (70%)  1016 (100%)  7.9  2.7  1.2  1.1  2.6  1.4  Industrial Planner Motion Time [s] Quintic Planner Motion Time [s]  '•'  7.2  •  . .  1.2  Industrial Planner Path Deviation RMS [mm]  0.5  0.5  0.6  0.6  Quintic Planner Path Deviation RMS [mm]  0.1 ,  0.2  0.3  0.4  Industrial Planner Maximum Path Deviation [mm]  1.5  1.5  1.6  1.5  Quintic Planner Maximum Path Deviation [mm]  0.5 [".  0.6  . 0.9  .  0.8  Table 7.6. Motion Times and RMS Errors of Task Space Experiment 1  7.2.1.2. Experiment 2: Task-Space Straight Line with Constant Orientation The example path of Section 7.2.1.1 provided a motion with a distance sufficiently large that the M M A S could be reached during the motion. The motion distance in Task Space Experiment 2 (Table 7.7) is limited such that the quintic planner limits the path speed attainable because of the shortened distance. That is, the M M A S cannot be reached, even if such a speed is demanded.  In Figure 7.6, the maximum speed attainable with the quintic planner is just above 400 mm/s, while that of the industrial planner is just below 1000 mm/s. In fact, it was seen that the industrial planner speed profile remained the same with demanded path speeds of 80% (813 mm/s), 90% (914 mm/s) and 100% (1016 mm/s) of the MMAS. That is, the high acceleration demanded for this motion causes overshoot of the demanded path speed (at 80 and 90 % MMAS). On the other hand, at these speeds, the quintic planner provides a trajectory that accelerates to the maximum attainable speed given the jerk and acceleration limits, before decelerating to zero speed at the end of the motion.  When the industrial planner is used, the path deviation increases substantially as the speed increases (Table 7.8, Figure 7.7). The quintic planner provides low average path deviations by respecting the speed, acceleration and jerk limits. However, both planners provide roughly equivalent path deviations at comparable motion times. As expected, the path deviation increases during a non-zero acceleration phase (Figures 7.8 and 7.9). The industrial planner trajectory was based on an acceleration which violated the M M A A for demanded speeds above 203 mm/s (30% MMAS). The maximum speed reached by the industrial planner trajectories during the motion is higher than that reached by the corresponding quintic planner trajectories. Consequently, the motion times corresponding to the industrial planner trajectories are smaller than those resulting from the quintic trajectories. However, large path deviations occur when the acceleration limits are exceeded (Figures 7.8(b) and 7.9(b)). Figure 7.10 shows that the quintic trajectories allow the controller to maintain the end effector orientation closer to the desired (constant) orientation than the industrial planner.  Chapter 7  79  102 (10%)  Demanded speed:  305 (30%)  508 (50%)  1000  1000  800  800  1016 mm/s (100%) (of MMAS)  a)"  1 600  1 600  -  T>  <D <B  <» 400  Q.  51 200 !<  ! '• • .  in  \ 1  400  W  200  i .\ i ; .  0 0  508 & 1016 mm/s  „.,,„!  0.5  t  _ „.„„  .  ,  r  1.5  1  time [s]  0  0.2  (a) Industrial Planner  0.4  0.6  time [s]  0.8  (b) Quintic Planner  Figure 7.6. Comparison of Speed During Motion of Task Space Experiment 2  102 305 (10%) (30%) reference path  Demanded speed:  360  430 425  320  £ 420  300  .» 415  e  N  280  360  430  430.5 - x-axis [mm]  431  V  430.5 . x-axis [mm]  431  A  395' 260  280  300 320 y-axis [mm]  340  435  w ^  430  (a) Industrial Planner 430  /508& 1016 mm/s  425 1=420  508 & I0l6mm/s  E  ^415  W  (  280 260  400 395' 429.5  431.5  'S3 <  § 300  410 405  340 •|320  1016 mm/s (100%) (of MMAS)  435  340  260' 429.5  508 (50%)  410  405 430.5 x-axis [mm]  431.5  400 395 429.5  430.5 x-axis [mm]  395'— 260  300 320 y-axis [mm]  (b) Quintic Planner Figure 7.7. Comparison of Tracking Performance of Task Space Experiment 2 (Note that x-axis scale has been magnified)  Chapter 7  80 Point  x [mrn|  y [mm]  z [mm]  yaw [rad]  pitch [rad]  roll [rad]  1  430  355  430  2n ~9~  71 360  71 90  ;r ~9~ 360 90 Table 7.7. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 2 2  430  265  400  2TT  3.5  Industrial Planner Quintic Planner  Industrial Planner -Quintic Planner  3  0.1  0.4 0.6 time [s]  (a) Demanded Speed: 102 mm/s (10% MMAS)  0.2 0.3 time [s]  0.4  0.5  (b) Demanded Speed: 1016 mm/s (100% MMAS)  Figure 7.8. Comparison of Path Deviation During Motion of Task Space Experiment 2  o x 10  3000  Industrial Planner Quintic Planner  2000 „1000  1  £  o o  o  E, 0 0  •  \- r - - - - f - - - ^ V \ / V ^ — f  (S  as  a -a  0.2  0.4  0.6 time [s]  0.8  1  (a) Demanded Speed: 102 mm/s (10% MMAS)  " 0  max max  -1  -1000 -zuuu 0  Industrial Planner Quintic Planner  \  CNI </>  \  4  c.  0.1  0.2 0.3 time fsl  0.4  0.5  (b) Demanded Speed: 1016 mm/s (100% MMAS)  Figure 7.9. Comparison of Acceleration Profiles (filtered) of Task Space Experiment 2 (Note different acceleration scales)  Chapter 7  81  102(10%)  305 (30%)  508 (50%)  1016(100%)  Industrial Planner Motion Time [s]  1.1  0.3  Quintic Planner Motion Time [sj  1.0  0.4 0.5  0.5  0.2 0.5  Industrial Planner Path Deviation RMS |mm|  0.1 0.2  0.3  0.5  1.6  0.3  0.3  0.3  0.3 0.6 0.8 Quintic Planner Maximum Path Deviation |mm| 0.4 0.5 (1.5 Table 7.8. Motion Times and RMS Errors Corresponding to Task Space Experiment 2  3.2 0.5  Demanded Speed Along Path |mm/s (MMAS)|  Quintic Planner Path Deviation RMS | iimi| Industrial Planner Maximum Path Deviation [mm)  102 (10%)  Demanded speed:  305 (30%)  508 (50%)  1016 mm/s (100%) (of MMAS)  0.7051  r 0.695 o  a:  i Vi  r 0.01  IV/ '  0.69  0  0.005  0.2  0.4 0.6 OS time [s]  0.4 0.6 OE time [s]  (a) Industrial Planner 0.02  ^  3 1 0 7  ^  ^  ^  3.106 0.69 v  0.685,  0.2  0.005  508& 1016 mm/s 0.4 0.6 time [s]  0.8  1  0  508 & 1016 0.2  0.4 0.6 time [s]  508 & 1016  mm/s 0.8  1  3.103  0  0.2  mm/s  0.4 0.6 OE time [s]  (b) Quintic Planner Figure 7.10. Comparison of Orientation With Respect to World Frame During trie Motion for Task Space Experiment 2  Chapter 7  82  7.2.1.3. Experiment 3: Task Space Motion with a Change in Orientation To analyze a motion with a change in orientation an axis-angle representation is utilized. An orientation motion will be smooth if the derivatives of the angle trajectory, $7), are continuous. As explained in Chapter 5, the quintic planner uses a quintic trajectory for $/) with zero speed and acceleration end conditions between two way-points. The axis of rotation is held constant during the motion. Limits are placed on $7) and its derivatives. As shown in Figure 7.11, for a motion which only requires a change in orientation (Table 7.9), the industrial planner simply computes one motion at the manufacturer limits. That is, it does not respect the demanded speeds. The quintic planner, on the other hand, respects the demanded speed and also provides a much smoother <p{t) profile. However, in this example, the quintic planner motion time is always higher than that of the industrial planner, due to the imposed and respected acceleration limits. One measure of smoothness for orientation is the amount of "wobble" associated with the motion. That is, for a motion described by an axis-angle representation, how much the path orientation deviates from this axis. The objective is to have a smooth motion about a single axis for operations such as pouring liquids or turning nuts or screws. Table 7.10 shows the angle between the desired axis of rotation and the actual axis of rotation, given at time increments along the path. The industrial planner motion clearly does not rotate about the desired axis, while the quintic planner motion does, although the angle between the desired and actual axis increases as the maximum path speed is increased.  102 (10%)  Demanded speed:  305 (30%)  711 (70%)  1016 mm/s (100%) (of MMAS)  3 2.5  2 1.5  • all speeds  r-,  2  2  1.5  305 mm/s 02 mm/s 0.5 0.  1  2  3 time [s]  4  (a) Industrial Planner  2  3 time [s]  4  (b) Quintic Planner  Figure 7.11. Comparison of cf>(t) at Different Maximum Allowable Speeds  Chapter 7  83  Point x [mm]  y [mm]  z [mm]  yaw [rad] n "ii  1  590  -325  220  2  590  -325  220  pitch [rad] roll [rad] 7t n ~12 T80  n  Table 7.9. Coordinates of Orientation Motion Start and End Points Industrial Planner  It  n  20  200  Quintic Planner  Demanded Speed Along the Path [mm/s (MMAS)]  all speeds  102 (10%)  305 (30%)  711 (70%)  1016 (100%)  Motion Time [s]  0.3  4.3  1.5  0.7  0.5  0.012 0.0095 0.0067 0.0058  0.012 0.010 0.0087 0.0083  0.0035 0.0024  0.0042 0.0035  Angle Between the Desired Rotation Axis and the Actual Rotation Axis [rad] at  max. min. max. min.  0.12 0.11  0.0043 0.0016  0.0088 0.0062  0.077 0.058  0.0012 0.00045  0.0033 0.0021  75%  max. min.  0.040 0.014  0.0010 0.00062  100%  max. min.  0.0032 0.00097  25% 50%  for Task Space Experiment 3  0.0012 0.00033 0.00080 0.0014 0.00011 0.0013 Axis and the Actual Rotation  0.0016 0.0018 0.0012 0.0013 Axis at Time Increments  7.2.1.4. Summary of Results for Task Space Stop Point to Stop Point Motions The three experiments discussed above, as well as an analysis of data for four other paths (Appendix H, Section H.l) demonstrate that the quintic planner provides faster motions at lower demanded path speeds, with lower path deviations. The shorter motion times are due, in part, to the industrial planner not reaching the acceleration limit. It was seen that when, for a given demanded speed, the industrial planner provided a faster motion, it did so by violating the manufacturer maximum allowable acceleration (Appendix H, Section H.l). The quintic planner also provides the ability to limit the kinematic parameters during a change in orientation while the industrial planner does not. Comparison of maximum path deviations at comparable motiontimesfor showed that in most cases, the quintic planner provided considerably improved maximum path deviation (ranging from 14% to 47% improvement). In one case, the quintic planner provided poorer maximum path deviation at comparable motion times, because the acceleration limit was too high for the given task-space path (refer to Section  Chapter 7  84  H. 1.6.1, Appendix H). However, in all cases, implementation of the quintic planner trajectories resulted in lower maximum path deviations at a demanded speed of 1016 mm/s (100% MMAS). That is, use of the quintic planner guarantees a lower maximum path deviation at the maximum demanded speed. The same conclusions apply to the average path deviations. In summary, for point-to-point motions, the quintic planner provides a motion that respects the kinematic limits, optimizing the trajectory within these limits. At low demanded speeds, the quintic planner provides a faster motion since it reaches the maximum acceleration possible given the velocity and jerk constraints. At high speeds the quintic planner provides a slower motion than the industrial planner because the industrial planner violates the acceleration constraints, resulting in a trajectory that causes large path deviations when the controller tries to track it. 7.2.2. Task-Space Multiple Way-Point Motion A multiple way-point task-space motion example is provided in this section. The way-points are given in Table 7.11. For the industrial planner, as the maximum allowable speed increases, the path shape is modified, i.e., the blend radius increases (Figure 7.12(a)). The quintic planner, on the other hand, generates the same path regardless of the demanded speed (Figure 7.12(b)). At all demanded speeds, the quintic planner provides a faster overall motion (Table 7.12).  Point  x [mm]  y [mml  ' [mm]  1  315  -300  390  2  470  -300  390  3  470  300  390  4  315  300  390  5  315  -300  390  yaw [rad]  pitch [rad]  5K  K  9  ~2  18  5K  K  9  2  11/r 18  5K  K  \\K  9  ~2  5K  K  9  ~2  5K  K  9  ~2  roll [rad] \\K  18 \\K  18 \\K  18  Table 7.11. Coordinates of Way-Points Corresponding to Figure 7.12. The fact that the quintic planner path is invariable over a range of demanded speeds is important in that it provides a predictable motion. When the industrial planner is used to design a motion at low speeds that is later run at a high speed, the path changes and the manipulator end effector may subsequently contact an object in the workspace.  Chapter 7  85  As shown in Figure 7.13, at a demanded path speed of 102 mm/s (10% MMAS), there is only a slight decrease in speed at the third and fourth way-points when the industrial planner is used. The corresponding acceleration and jerk components will be very large. The quintic planner provides limited jerk and acceleration when cornering (or blending). The difference between the motion times of the industrial and quintic trajectories increases as the speed increases due mainly to the higher speeds attained along the straight line segments with the quintic trajectories. It was also found that the industrial planner motion time increased slightly when the maximum path speed was increased to 90 (914 mm/s) and 100% (1016 mm/s) MMAS (refer to Section H.2.1, Appendix H, for complete results). A second multiple way-point example is provided in Section H.2.2, Appendix H, and corroborates the above conclusions.  Demanded Speed Along the Path |mm/s (% MMAS)|  102(10)  406 (40)  508 (50)  1016(100)  Industrial Planner Motion Time |s|  15.12  4.75  4.17  3.90  14.96 4.13 Table 7.12. Motion Times for Task Space Multiple Way-Point Path  3.66  2 99  Quintic Planner Motion Time |s|  Demanded speed:  102  406 (40%)  (10%)  508 (50%)  1016 mm/s (100%) (of MMAS)  way-point 400  400  200  200  -200 -400 300  -200  AA 350  400 x axis [mm]  450  (a) Industrial Planner  500  -400 300  350  400 x axis [mm]  450  (b) Quintic Planner  Figure 7.12. Comparison of Paths at Different Speeds for Task Space Multiple Way-Point Path  500  Chapter 7  86  Demanded speed:  406 (40%)  102 (10%)  508 (50%)  1200  1200  1000  1000  «f 800 E  f  600 CD <D « 400 200  M  |  800  f  600  <u cu  Q_ , „  « 40l  mt  200  1.  '•V*'<f"  0U 0  1016 mm/s (100%) (of M M A S )  \\ , \ 5 . . . . 10 time [s]  0  15  5  (a) Industrial Planner  time [s]  10  15  (b) Quintic Planner  Figure 7.13. Comparison of Speed Profiles for Task Space Multiple Way-Point Path  7.2.3. Joint-Space Stop Point to Stop Point Motion For stop point to stop point motions in joint space, the most important considerations are the motion times and trajectory end-point accuracy. It is not important that the joint space path be followed accurately since this type of motion is used to move rapidly between way-points when the robot is transiting between tasks (It is, however, important that the path be invariable with path speed). Thus, in this section, the difference between the actual and desired joint positions at the end of the motion is used as a comparison metric to determine how accurately the way-points are reached.  7.2.3.1. Experiment 1: Joint Space Straight Line For the joint space motion between the two points given in Table 7.13, the quintic planner provides a faster motion than that of the industrial planner at all demanded speeds (Table 7.14, Figure 7.14). The industrial planner provides a slower motion because it uses a lower deceleration limit than acceleration limit, while the quintic planner uses the same acceleration and deceleration limit (Figure 7.15).  Point i  i  Joint 1 [rad]  In 18 AJT  Z  Joint 2 [rad]  Joint 3 [rad]  Joint 4 [rad]  Joint 5 [rad]  13TZ-  n  In  9  18  ~6  ~9  5n 9  1 \7l  71  71  71  Joint 6 [rad] 5TT  "78 l7T  ~9~ 18 ~6 7 Table 7.13. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 1  Chapter 7  87  (a) Demanded Speed: 102 mm/s (10% of M M A S ) 0.1  joints 3 & 6  joints 3 & 6  joints 2, 4 & 5  _ .  \ -joints 2, 4 ^ &5  0 2  -joint I  joint 1 Time [s]  (b) Demanded Speed: 305 mm/s (30% of M M A S ) joints 3 & 6 joint 5 joint 2 joint 4  1  •joints 3 & 61  -0.2 =3 (U  >. o o  •joint 5 •joint 2  -0.4  5£  -0.6  -joint 4 joint 1  -0.8  joint 1  -1  l/VVvwv  " o 12  Time [s]  (c) Demanded Speed: 711 mm/s (70% of M M A S ) 0.5  joints 3 & 6 joint 5 joint 2 joint 4 joint 1  joints 3 & 6 I  0  CA  >. "o o CD  >  -0.5 -1 -1.5 -2  - o 25  0  Time[s]  5  1  (d) Demanded Speed: 1016 mm/s (100% of M M A S ) -joints 3 & 6  joints 3 & 6  0  (a) Industrial Planner  5  Time[s]  1  (b) Quintic Planner  Figure 7.14. Comparison of Joint Velocities for Joint Space Experiment 1  Chapter 7  88  Both the quintic and industrial planner trajectories cause the manipulator to reach the end of the trajectory with equivalent accuracy for comparable motion times (Table 7.14). In both cases, the accuracy with which the end point is reached decreases with increasing demanded speeds (refer to Section H.3.1, Appendix H for complete results).  Industrial Planner Demanded Speed Along Path [mm/s ( M M A S ) l  Quintic Planner 102  8.5  Motion Time [s]  (10%)  8.4  305  (30%)  3.1  0.001 0 ti 2[radl 0 0.001 0.001 Deviation 3 [radl 0 0.001 0 of Joint 4[radl 0 0 0.002 0 5[radl 0.001 0.002 0.001 6[rad] 0.001 0.001 Table 7.14. Motion Times and Accuracy with which Experiment 1 1[rad]  2.9  711  (70%)  1.6  0 0 0.001 0.001 0.001 0.001 0.003 0.00' 0.002 0.003 0.001 0.001 the End Point is  1016 (100%)  1.4  1.4  1.1  li  0 0.001 0.001 0.004  0  0.002 0.001 0.004  0.002 0.001 OOiM  o.oiM 0.003 0.001 0.001 0.001 Reached for Joint Space 0 (l()4  Figure 7.15. Comparison of Industrial Planner and Quintic Planner Acceleration Profile (filtered) for Joint Space Experiment 1 Trajectory with Demanded Speed of 1016 mm/s (100% MMAS)  Chapter 7  89  7.2.3.2. Experiment 2: Joint Space Straight Line The second joint space experiment was for a shorter distance, such that the peak speed was limited by end-point requirements (Table 7.15). The quintic planner did not allow the speed to increase once the demanded speed was above 508 mm/s (50% MMAS). The industrial planner actually produced motions that were slower when higher path speeds were demanded (Figure 7.16, Table 7.16). In fact, with the industrial planner, for a demanded speed of 1016 mm/s (100% MMAS) the motion time is actually larger than that for a demanded speed of 102 mm/s (10% MMAS). This occurs when a distance limitation is present, as explained in Section H.3.2 (Appendix H).  Point 1  Joint 1 [rad]  Joint 2 [rad]  Joint 3 [rad]  it  n  17*  ~~9  7  18  5*  5*  33*  Joint 4 [rad]  Joint 5 [rad]  *  0  ~~9  *  it  *  Joint 6 [rad] 0  *  36 18 36 ~18 ~12 18 Table 7.15. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 2  Demanded Speed Along the Path [mm/s (MMAS)]  102(10%)  305 (30%)  711 (70%)  1016(100%)  Industrial Planner Motion Time [s]  0.8  0.6  0.8  0.9  Quintic Planner Motion Time [s]  0.7  0.3  0.3  0.3 Table 7.16. Motion Times for Straight Line for Joint Space Experiment 2  7.2.3.3. Summary of Joint Space Stop Point to Stop Point Motion Results The complete results for joint space stop point to stop point motion, as well as two further examples can be found in Section H.3, Appendix H. The quintic planner provides faster motions than the industrial planner at all demanded speeds because the industrial planner utilizes a lower deceleration and jerk than acceleration and jerk.  The method used by the industrial planner gives poor results over small motion distances. An increase in demanded speed results in an unexpected and undesirable increase in motion time. The quintic planner, on the other hand, reaches the demanded speed or the highest speed possible given the motion distance and kinematic limits.  Chapter 7  (a) Demanded Speed: 102 mm/s (10% of M M A S ) 0.5i  .  joints 2 & 6  joints 2 & 6  joint 5 joints 1 & 3 joint 4 0.4 0.6 Time [s]  -joints 1 & 3  0.8  0.4 0.6 Time [s]  0.8  (b) Demanded Speed: 305 mm/s (30% of M M A S ) joints 2 & 6  -joints 2 & 6  ioint 5 -joint 5  0.2  joints 1 & 3  -joints 1 & 3  joint 4  -joint 4  0.4 0.6 Time [s]  0.8  1  0.2  0.4 0.6 Time [s]  0.8  (c) Demanded Speed: 711 mm/s (70% of M M A S ) 15,  1.5 1 3T 0.5  .  -joints 2 & 6  joints 2 & 6 -joint 5  -joint 5  _  5u  o  o  § -0.5 -1 -1.5'  joints 1 & 3  -joints 1 & 3  joint 4 0.2  -joint 4 0.4 0.6 Time [s]  0.8  1  0.2  0.4 0.6 Time [s]  0.8  (d) Demanded Speed: 1016 mm/s (100% of M M A S ) 1.5 1  joints 2 & 6  0.5  -joints 2 & 6 joint 5 -joint 5 joints 1 & 3  -1 -1.5  -joints 1 & 3 -joint 4  joint 4 0.2  0.4 0.6 Time [s]  0.8  (a) Industrial Planner  0.4 0.6 Time [s]  1  (b) Quintic Planner  Figure 7.16. Comparison of Joint Velocities for Joint Space Experiment 2  1  Chapter 7  9J.  With the imposed kinematic parameters, all the joint velocity profiles utilizing the quintic planner exhibit oscillations during the first part of the speed cruise region. In addition, the resulting acceleration profiles slightly overshoot the acceleration limits. An example is provided below. Since this is most visible for the joint that reached the highest path speed, the acceleration of joint 1 for the joint space experiment 1 at a demanded speed of 305 mm/s (30% M M A S ) is shown in Figure 7.17(a).  As shown in Figure 7.17 (b), when the joint jerk limits are reduced such that the acceleration ramp time is equal to 150 ms, the resulting experimental acceleration profile follows the reference acceleration profile much more closely. In addition, the magnitude of the oscillations in the velocity profile is reduced (Figure 7.18, most visible in joints 1 and 2).  With the original joint jerk limits, the time allowed for a ramp to maximum acceleration,  dt , is 20 ms. max  As shown in Figure 7.17(a), during the ramp down to minimum acceleration, the reference signal is not followed due either to controller or actuator saturation. However, when the jerk limit was reduced (Figure 7.17(b)), the acceleration ramp was followed more closely. This indicates that the manufacturer joint jerk limits are set too high. That is, the manipulator was unable to follow the trajectory signal or the controller was saturated. Further work, beyond the scope of this thesis, is required to determine the correct joint jerk limits necessary to avoid controller saturation. This will require integrated monitoring of the reference signal, which cannot currently be done from the encoder debugging header provided on the C500C controller.  experimental  reference 15  "7 10 ra  A  5  o ra JE  0  < -5  -a. 0.5  (a) dt  1 nmx  Figure 7.17.  1.5 Time [s]  2  |-10  2.5  = 20 ms (high jerk)  if 0.5  -15.  (b) dt  1 max  1.5 Time [s]  2  2.5  = 150 ms (low jerk)  Comparison of Filtered Joint Acceleration Profiles for Different Jerk Limits (Joint Space Experiment 1, demanded speed of 305 mm/s (30% MMAS))  Chapter 7  92  0.4  ^joints 3 & 6  0.2  //joints 3 & 6  joint 2  joint 0.5  1  (a) dt  max  Figure 7.18.  1.5 2 Time [s]  2.5  0.5  (b) dt  1  = 20 ms (high jerk)  max  1.5 Time [s]  2  2.5  = 150 ms (low jerk)  Comparison of Joint Velocity Profiles With Different Jerk Limits (Joint Space Experiment 1, demanded speed of 305 mm/s (30% MMAS))  7.2.4. Joint Space Multiple Way-Point Motion A joint space motion between the points given in Table 7.17 demonstrates that the quintic planner provides a consistently faster motion between the way-points at all maximum path speeds (Table 7.18). The industrial planner path was modified as the demanded speed was increased; the blend size increasing with demanded speed (Figure 7.19(a)). The quintic planner path remained constant with increasing speed (Figure 7.19(b)). At low demanded speeds, the industrial planner path will require large jerks due to the sharp blends.  Point 1  z i  j A  Joint 1 [rad]  jt '  ~6  Joint 2 [rad] 2K  ~T  Joint 3 [rad] 10a-  Joint 5 [rad]  Joint 6 [rad]  0  0  0  9  n  IK  \1K  T  18  n  IK  3  Ts  K  5K  7  18  K  Joint 4 [rad]  7  27T  K  K  K  18  ~9  18  ~9  17a-  K  K  K  ~9  18  ~9  K  2TT  18 rc  9 10a  0 0 6 9 Table 7.17. Coordinates of Way-Points for Multiple Way-Point Joint Space Path 5  T  \\K  18 0  Chapter 7  93  Demanded Speed Along the Path [mm/s (MMAS)|  102(10%) 305 (30%)  711 (70%) 1016(100%)  7.31 21.32 Quintic Planner Motion 1 inie |s| 21.21 - IS Table 7.18. Motion Times for Multiple Way-Point Joint Space Path  3.47 1.28  Industrial Planner Motion time |s|  Demanded speed: P  1 0 2  (10%) • way-point  joint 2 [rad]  1  0  1  6  m  m  /  2.88  s  (100%) (of MMAS)  joint 4 [rad]  (a) Industrial planner  joint 2 [rad]  joint 4 [rad]  (b) Quintic planner Figure 7.19. Comparison of Joint Space Paths for Joint Space Multiple Way-Point Experiment (plots chosen for best view of blends)  Chapter 7  94  7.3. Comparison to SPCTOM It has been shown that Smooth Path-Constrained Time-Optimal Motion (SPCTOM) with low torque rate limits results in a faster and more accurate motion than an optimal trajectory that does not incorporate torque rate limits [14]. The quintic concatenation method was compared to the SPCTOM with low torque rate limits using the positional dof of a SCORBOT ER VII. The path followed, described in parametric form, is x(d) = 400 mm y(o) = 3005- 100 mm  (7.1)  z(6) = 200s + 300 mm 0=0,..., 1. The SPCTOM trajectory was designed for torque saturation limits of 10 N.m and torque rate limits of 10 N.m/s for each joint. The quintic concatenation trajectory was generated with maximum allowable speed, v , of 370 mm/s, max  maximum acceleration, a , of 890 mm/s , and jerk limit, j„ , of 4450 mm/s , resulting in similar torque 2  3  max  wx  and torque rate demands. As shown in Figures 7.20, 7.21 and Table 7.19, the two methods are comparable. Thus, the quintic concatenation method, although slightly slower, provides a smooth trajectory, which has the same properties as the SPCTOM, but is computationally simpler and therefore faster to generate. The large RMS tracking errors are due, in part, to controller model errors and the control scheme being used, namely, independent joint control.  Trajectory  Quintic Concatenation  Motion Time [s] 1.7  RMS Path Deviation [mm]  Maximum Path Deviation [mm]  0.028  5  8  0.026  4  8  RMS Tracking Error [rad]  jt3  jtl 0.039  0.039  0.034 1.5 0.042 SPCTOM Table 7.19. Motion Times for SPCTOM Trajectory  Chapter 7  95  time[s]  time[s]  time [s)  Figure 7.20. Comparison of Quintic Concatenation and SPCTOM Joint Performance  7.4. Computation Time Issues The typical computation time for both the industrial and quintic trajectories is on the order of 1 ms for a point to point trajectory. The computation time for a point to point smooth path-constrained time-optimal motion is on the order of 30 s; that is, four orders of magnitude greater than that of the kinematic methods presented. Most importantly, dynamic methods generally involve optimization and therefore do not have a fixed computation time. The quintic planning method described herein is unique among the jerk-limited motion planning schemes reviewed in Chapter 2 in that it demonstrates an analytical solution approach that does not require any iterative calculations. That is, it has a fixed and known computation time, and has been implemented on-line in an industrial controller setting. 7.5. Summary In this chapter the quintic planner was compared to an industrial planner and to a smooth, time-optimal motion planner. The industrial and quintic planner trajectories were tracked using the same controller.  Chapter 7  96  It was found that, in task space stop point to stop point motions, the quintic planner provided faster motion and decreased average and maximum path deviation at low demanded path speeds. At higher demanded path speeds, the industrial planner provided faster motions but poorer path deviations at similar motion times since the trajectories violated the acceleration limits. For multiple way-point task space motions, the quintic planner provides consistently faster motion times and a constant motion path. The industrial planner trajectories are slower and vary the path with the maximum demanded path speed. In joint space, the quintic planner provided consistently faster point-to-point and multiple way-point motions since the industrial planner uses a lower deceleration than acceleration. In distance-limited cases, as the demanded path speed was increased, the industrial planner produced increasingly slower motions. The quintic planner always provides motion times that decrease or remain constant as the demanded speed is increased. Comparison to a SPCTOM trajectory showed that the quintic trajectories result in slightly poorer tracking accuracy and slightly longer motion time. However, the quintic trajectories can be generated on-line, while the SPCTOM trajectories must be implemented off-line.  Chapter 8 Conclusions & Recommendations 8.1. Conclusions In this thesis, a smooth, on-line, trajectory generation method has been presented. The trajectory path consists of straight lines interconnected with geometric blends, which pass a specified distance from the way-points. The smoothness of the trajectories is obtained through appropriate selection of the jerk limits, and planning trajectories that respect these limits. The straight-line trajectories are formed using concatenated quintic segments that impose C continuity on 2  the trajectory and limit the velocity, acceleration and jerk during the trajectory. In addition, undesirable oscillations in the position and velocity domains are avoided. The blend function used to connect the straight line trajectories also imposes C continuity, and a method as been proposed herein to limit the 2  acceleration and jerk during the blend. Both the straight-line and blend trajectories have zero acceleration end conditions. Thus, by design, the overall trajectory has a continuous acceleration profile along with limited velocity, acceleration and jerk. Utilizing conservative, but reasonable kinematic limits, this approach provides an efficient, near timeoptimal, on-line planning method. The ability to plan in both task and joint space is provided. The task space motion can be used in tasks when the path followed by the manipulator end effector is of importance. Joint space motion can be used when transiting between tasks, in areas of the workspace where no obstacles are present, since joint-space motion provides a faster motion between any two way-points than task-space motion. A high level system that allows trajectories between multiple way-points to be planned on-line was also developed. The trajectory is planned by calculating quintic control points (up to eight per way-point to way-point trajectory) and the blend parameters, which are used at a later stage, by the parser, to generate the controller reference signal.  97  Chapter 8  98  The trajectory generation method was implemented using a typical industrial controller. Comparison of the quintic planner to an industrial planner with no jerk limitations showed that the quintic planner provided faster motions in joint space with equivalent end point accuracy. In task space, the quintic planner provided an overall reduction in maximum and average path deviations. In addition, the quintic planner provides trajectories that consistently utilize the maximum possible acceleration and speed. In task space, as the demanded speed is increased, the industrial planner also increases the acceleration, eventually violating the manufacturer maximum allowable acceleration. As the demanded speed is increased, the quintic planner provides trajectories with decreased or constant motion times. The industrial planner, on the other hand, provides increased motion time with increasing demanded speed in joint space when the planned motion is short. The successful implementation of the quintic concatenation trajectories on a commercial industrial controller demonstrates that the method requires a computational load low enough for use on-line. In addition, the method is compared to a smooth dynamic-based trajectory, the smooth, path-constrained, time-optimal motion (SPCTOM), and it is shown that similar, if slightly poorer, motion times and tracking accuracy are obtained through use of the quintic trajectories. However, the quintic trajectories can be planned and generated much more rapidly than the SPCTOM trajectories. 8.2. Recommendations This research has provided an improved method of calculating trajectories on-line, which can be used to avoid controller saturation and the resulting wear on the machine by appropriate selection of the jerk limits. This methodology can easily be extended to accommodate other basis functions. However, there are a number of improvements that can be explored. These, as well as a number of research directions, are discussed below. (1) The orientation motion quintic trajectory could be replaced by a concatenated quintic trajectory when the orientation motion time is limiting. However, this would lead to a more iterative algorithm. (2) The trajectory generation methodology provided herein for straight-line paths was based on minimizing the jerk discontinuities. However, the blends used to join the straight-line segments cause large discontinuities in jerk at the blend start and end points. The basis function (sine wave template) could be modified to allow a bang-bang jerk profile, providing a faster motion time but resulting in even larger jerk discontinuities. This change would only require the modification of the quintic end conditions and the trajectory would in fact become a concatenation of cubics, similar to the work done by Castain and Paul [9]. However, no iterative calculations would be required if the method  Chapter 8  [  99  proposed herein were used, and the motion time would be calculated directly by the planner. Alternatively, the blend functions could be modified in order to decrease the size of the jerk discontinuity. (3) The above considerations lead to another area that should be investigated: the role of snap (the derivative of jerk with respect to time) in trajectory generation. It was found that the SCORBOT-ER VII manipulator deviated greatly from the trajectory shortly after the start of the blends, most often at high speeds. This may be due to the large change in jerk that occurs at these points. However, designing a trajectory with limited snap would increase the motion time. (4) One could also investigate the incorporation of local kinematic limits, based on the robot configuration and its expected joint velocities and accelerations. This would allow the kinematic limits to be optimized for the given robot configuration, increasing the motion time and tracking accuracy of the manipulator. (5) Finally, when switching between joint space planning and task space planning, it is necessary to stop the manipulator at the intermediate way-point. Motion time would be improved with the ability to switch from task space planning to joint space planning or vice-versa without requiring the manipulator to stop. The investigation of these topics would further contribute to the area of on-line trajectory planning.  Bibliography [I] Andersson, R., L., A Robot Ping-Pong Player: Experiment in Real-Time Intelligent Control, Cambridge, MIT Press (1988). [2] Angeles, J., Alivizatos, A., Zsombor-Murray, P., "The Synthesis of Smooth Trajectories for Pickand-Place Operations", IEEE Transactions on Systems, Man, and Cybernetics, Vol. 18, No 1.,  (1988): 173-178. [3] Bazaz, S., Tondu, B., "3-Cubic Spline for On-Line Cartesian Space Trajectory Planning of an Industrial Manipulator", International Workshop on Advanced Motion Control, (1998): 493-498. [4] Bobrow, J. E., Dubowsky, S., Gibson, J. S., "Time-Optimal Control of Robotic Manipulators Along Specified Paths", International Journal ofRobotics Research, Vol. 4, No. 3, (1985): 3-17. [5] Brock, S., Kaczmarek, T., "2 D Command Preprocessor with Jerk Limit for Machine Tool Drives", IEEE International Symposium on Industrial Electronics, Vol. 1, (1996): 230-235. [6] C500C Controller User Guide, CRS Robotics, 2000. [7] Cao, B., Dodds, G. I., "Time-Optimal and Smooth Joint Path Generation for Robot Manipulators", Proceedings of the International Conference on Control, (1994): 1122-1127. [8] Cao, B., Dodds, G. I., Irwin, G. W., "Time-Optimal and Smooth Constrained Path Planning for Robot Manipulators", Proceedings of the IEEE International Conference on Robotics and Automation, (1994): 1853-1858. [9] Castain, R. H., Paul, R. P., "An On-Line Dynamic Trajectory Generator", The International Journal of Robotics Research, Vol. 3, No. 1, (1984): 68-72. [10] Chand, S., Doty, K.L., "On-Line Polynomial Trajectories for Robot Manipulators", International Journal ofRobotics Research, Vol. 4, No. 2, (1985): 38-48. [II] Chang, Y.-H., Lee, T.-T., Liu, C.-H., "On-Line Approximate Cartesian Path Trajectory Planning for Robotic Manipulators", IEEE Transactions on Systems, Man, and Cybernetics, Vol. 22, No. 3,  (1992): 542-547. [12] Chen, Y. B., Desrochers A. A., "Structure of Minimum-Time Control Law for Robotic Manipulators With Constrained Paths", Proceedings of the IEEE International Conference on Robotics and Automation, (1989): 971-976. [13] Chen Y. B., Desrochers A. A., "Time-Optimal Control for Two-Degree of Freedom Robot Arms", IEEE Transactions on Robotics and Automation, Vol. 6, No. 3, (1990): 1210-1215. [14] Constantinescu, D., Smooth Time Optimal Planning for Industrial Manipulators, M.A.Sc, Department of Mechanical Engineering, University of British Columbia (1998).  100  Bibliography  101  [15] Constantinescu, D., Croft, E. A., "Smooth and Time-Optimal Trajectory Planning for Industrial Manipulators Along Specified Paths", Journal ofRobotic Systems, Vol. 17, No. 5, (2000): 233-249. [16] Croft, E. A., Benhabib, B., Fenton, R.G., "Near-Time Optimal Robot Motion Planning for On-Line Applications", Journal of Robotic Systems, Vol. 12, No. 8, (1995): 553-567. [17] Dahl, O., Nielsen, L., "Torque-Limited Path Following by On-Line Trajectory Time Scaling", IEEE Transactions on Robotics and Automation, Vol. 6, No. 5, (1990): 554-561. [18] Dahl, O., "Path-Constrained Robot Control with Limited Torques - Experimental Evaluation", IEEE Transactions on Robotics and Automation, Vol. 10, No. 5, (1994): 658-669. [19] Driedger, K., Letter, "Requirements Document: Robot Motion and Trajectory Planning", CRS Robotics (January 10, 2000). [20] Erkorkmaz, K., High Speed Contouring Control for Machine Tool Drives, M.A.Sc. Thesis, Department of Mechanical Engineering, University of British Columbia (1999). [21] Erol, N.A., Altintas, Y., "Open Architecture Modular Tool Kit for Motion and Process Control", ASME International Mechanical Congress and Exposition, ASME Publication MED, Vol. 6-1 (1997): 15-22. [22] Fang, Y. C , Hsieh, C. C , Kim, M. J., Chang, J. J., Woo, T. C , "Real Time Motion Fairing With Unit Quaternions", Computer-Aided Design, Vol. 30, No. 3, (1998): 191-198. [23] Fisher, W. D., Mujtaba, M . S., "Minimum Ratio-Locked Profile Times for Robot Trajectories", Proceedings of the IEEE International Conference on Robotics and Automation, Vol. 2, (1988): 1054-1060. [24] Geering, H. P., Guzzella, L., Hepner, A. R., Onder, C. H., "Time-Optimal Motions of Robots in Assembly Tasks", IEEE Transactions on Automatic Control, Vol. AC-31, No. 6, (1986): 512-518. [25] Gosselin, C , Hadj-Messaoud, A., "Automatic Planning of Smooth Trajectories for Pick-and-Place Operations", Advances in Design Automation, Vol. 2, (1991): 531-536. [26] Kahn, M.E., Roth, B., "The Near-Minimum-Time Control of Open-Loop Articulated Kinematic Chains"', Journal of Dynamic Systems, Measurement, and Control, Vol. 93, (Sept. 1971): 164-172. [27] Kieffer, J., Cahill, A. J., James, M. R., "Robust and Accurate Time-Optimal Path-Tracking Control for Robot Manipulators", IEEE Transactions on Robotics and Automation, Vol. 13, No. 6, (1997): 880-890. [28] Kim, B. K., Shin, K. G., "Minimum-Time Path Planning for Robot Arms and Their Dynamics", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-15, No. 2, (1985): 213-223.  [29] Kyriakopoulos, K. J., Saridis, G. N., "Minimum Jerk Path Generation", Proceedings of the IEEE International Conference on Robotics and Automation, (1988): 364-369. [30] Leahy, M. B., Saridis, G. N., "Compensation of Unmodeled PUMA Manipulator Dynamics", IEEE International Conference on Robotics and Automation, (1987): 151-156.  Bibliography  102  [31] Lin, C.-S., Chang, P.R., "Joint Trajectories of Mechanical Manipulators for Cartesian Path Approximation", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-13, No. 6,  (1983): 1094-1102. [32] Lin, C. S., Chang, P. R., Luh, J. Y. S., "Formulation and Optimization of Cubic Polynomial Joint Trajectories for Industrial Robots", IEEE Transactions on Automatic Control, Vol. AC-28, No. 12, (1983): 1066-1074. [33] Lloyd, J., Hayward, V., "Trajectory Generation for Sensor-Driven and Time-Varying Tasks", International Journal ofRobotics Research, Vol. 12, No. 4, (1993): 380-393. [34] Miiller-Karger, C , Granados Mirena, A., Scarpati Lopez, J., "Hyperbolic Trajectories for Pick-andPlace Operations to Elude Obstacles", IEEE Transactions on Robotics and Automation, Vol. 16, No. 3, (2000): 294-300. [35] Paul, R. P., Robot Manipulators: Mathematics, Programming, and Control, MIT Press (1981). [36] Pfeiffer, F., Johanni, R., " A Concept for Manipulator Trajectory Planning", IEEE Journal of Robotics and Automation, Vol. RA-3, No. 2, (1987): 115-123. [37] Piazzi, A., Visioli, A., "Global-Minimum-Jerk Trajectory Planning of Robot Manipulators", IEEE Transactions on Industrial Electronics, Vol. 47, No. 1, (2000): 140-149.  [38] Press, W. H., Flannery, B.,P., Vetterling, W., T., Teukolsky, S., A., Numerical Recipes in C: The Art of Scientific Computing, Cambridge University Press, pp. 184-185, 1992. [39] Rajan, V. T., "Minimum Time Trajectory Planning", IEEE International Conference on Robotics and Automation, (1985): 759-764. [40] Red, E., " A dynamic Optimal Trajectory Generator for Cartesian Path Following", Robotica, Vol. 18, Part 5, (2000): 451-458. [41] Sahar G., Hollerbach, J.M., "Planning of Minimum-Time Trajectories for Robot Arms", The International Journal ofRobotics Research, vol. 5, no. 3, (1986): 90-100. [42] Sciavicco L., Siciliano, B., Modeling and Control of Robot Manipulators, McGraw-Hill Companies, Inc. (1996). [43] Shiller, Z., Chang, H., Wong, V., "The Practical Implementation of Time-Optimal Control for Robotic Manipulators", Robotics and Computer-Integrated Manufacturing, Vol. 12, No. 1, (1996):  29-39. [44] Shin, K. G., McKay, N. D., "Minimum-Time Control of Robotic Manipulators with Geometric Path Constraints", IEEE Transactions on Automatic Control, Vol. AC-30, No. 6, (1985): 531-541. [45] Shin, K. G., McKay, N. D., A "Dynamic Programming Approach to Trajectory Planning of Robotic Manipulators", IEEE Transactions on Automatic Control, Vol. AC-31, No. 6, (1986): 491-500. [46] Tam, H. Y., "Minimum Time Closed-Loop Tracking of A Specified Path by Robot", Proceedings of the 29' Conference on Decision and Control, (1990): 3132-3137. h  Bibliography  103  [47] Taylor, R. H., "Planning and Execution of Straight Line Manipulator Trajectories", IBM Journal of Research and Development, Vol. 23, No. 4, (1979): 424- 436.  [48] Tan, H. H., Potts, R. B., "Minimum Time Trajectory Planner for the Discrete Dynamic Robot Model With Dynamic Constraints", IEEE Journal of Robotics and Automation, Vol. 4, No. 2, (1988): 174185. [49] Tarkiainen, M . , Shiller, Z., "Time Optimal Motions of Manipulators with Actuator Dynamics", Proceedings of the IEEE Conference on Robotics and Automation, Vol. 2, (1993): 725-730. [50] Volpe, R., "Task Space Velocity Blending for Real-Time Trajectory Generation", Proceedings of the IEEE International Conference on Robotics and Automation, v.2, (1993): 680-687. [51] Ward, J.P., Quaternions and Cayley Numbers, Kluwer Academic Publishers (1997). [52] Yang, D.C.H., Chou, J.J., "Automatic Generation of Piecewise Constant Speed Motion with Smooth Transition for Multi-Axis Machines", Journal of Mechanical Design, Vol. 116, (June 1994): 581586.  Appendix A Jerk Considerations A quintic specified by the sine wave template, such as the one shown in Figure A. 1, has end conditions = P,  p{0)  (A-l)  t  P(dt )  maxdt max  a  max  + \ „,ax S dt  4  K  1  (A.2)  +P\'  (A.3)  = s,  s{0)  l  ^ 4 ^+  4dt x) = ma  (A.4)  s,,  fl(O) = 0,  (A. 5)  a(dt ) = 0,  (A. 6)  max  where pi is the initial position, s\ is the initial velocity, a  max  7tU  dt  „  2/ J  is the acceleration limit, and dt  max  is set to (A.7)  max  so that the jerk limit, j , will not be violated during an acceleration ramp. For the trajectory developed max  herein, formed of concatenated quintic segments, the largest jerk discontinuity occurs during an acceleration pulse. It will be shown below that the maximum jerk discontinuity is equal to 11% of the jerk limit.  Figure A. 1. Jerk, Acceleration, Velocity and Position Profiles Corresponding to a Sine-Based Acceleration Ramp from 0 to a max  104  Appendix A  105  The distance covered during an acceleration ramp from zero to the maximum acceleration, a„  based on  the sine wave profile is f  \  1^  4 TT  2  +  (A.8)  \ max-  S  dt  The quintic coefficients corresponding to this quintic are:  \  ^ /  4  xJ  3  (A. 10)  J ma  b = •5 + — v n J A  (A.9)  J ma  2  t 20 Jn b = 271 V 71  (A. 11)  i>2 — dmax,  (A. 12)  bl  (A. 13)  3  =S  h  (A. 14)  bo=Pl-  The equation corresponding to the jerk is: j(t) = 60b/ + 24b t + 6b  (A. 15)  Zit)=l20b t + 24b .  (A. 16)  4  h  and to the snap: s  4  Therefore, the maximum jerk may occur at either of the quintic extremities: (A.17)  t, = 0,  (A.18) or when ]di) = 0: b _ dt A  (A. 19)  max  5b,  It can be shown that at these points: (A.20)  i- 4 V n J 2  71  2  j{t )=60b dt +24b,dt +6b =2  2  5  ma  j{t )=l5b dt  ma  2  3  5  max  3  +\2b dt A  max  6 7T  20 ^ 2 - — 7 =-0.0505y ,,  +6b, = - 3 +  mat  60^  ma  = 0.980y„  (A.21) (A.22)  Appendix A  106  Therefore, the maximum jerk occurring during the acceleration ramp is equal to 0.980/ . In addition, the majr  jerk at the start and the end of the quintic is equal to -0.0505/„ . Thus, for a trajectory formed of a MV  concatenation of quintics, the worst discontinuity in jerk will occur during an acceleration pulse (a concatenation of two acceleration ramps). The magnitude of this discontinuity will be equal to 0.01 \j  max  or 11% of the jerk limit. However, as seen in Chapter 4, the jerk at the extremities of a blend can be equal to the jerk limit. Thus, when a straight-line segment is connected to a blend, there is a maximum jerk discontinuity of 1.05/ during the motion.  max  Appendix B Quintic End Conditions B . l . Quintic E n d Conditions for a Trajectory Based on a Sustained Acceleration Pulse The quintic end conditions (time, position, velocity, and acceleration) for a trajectory between two waypoints based on a sustained acceleration pulse are provided in Table B. 1. The quintic control points are shown in Figure B . l (taken from Chapter 3, cf. Figure 3.3). The Sustained Acceleration Pulse (SAP) algorithm is repeated below for clarity.  Sustained Acceleration Pulse Algorithm (s? > Si > 0) 1.  Calculate the total way-point to way-point distance, D = | | p - p ||.  2.  Calculate the two unknown control point speeds s„,  2  a  s„ =s, +  dt  max  {  (B.l)  max  and s/„ ® max ^ max d  3.  (B.2)  Calculate the distances covered during: - the ramp up to a , max  D,=a  dt  max  1  \_  \_  (B.3)  \ max-  +  max 4  S dt  K  2  the acceleration cruise,  si-si 2a„  D = 2  the ramp downfroma  mux  i  D  IF  (B.4)  to zero acceleration  = max lax a  ~ +  dt  U  (D,+D +D -D>0) 2  ~  (B.5)  h t ax •  +  S  d  m  K) 2  i  It is not possible to reach the demanded speed, s , due to the distance, acceleration and jerk limitations. Therefore, reduce 5/, and s so that the distance constraint is respected (i.e., reduce the length of the acceleration cruise): 2  2  ( b  S  ~  max t ax  a  d  \P max  +  m  max )  s =s 2  b  . 2  ^max^ ax m  max  2  j ,  -rs ai x  max  _  2a nax  max max  u  +  ~\.  D  (B.6)  j (B.7)  Calculate D and D from Eqs B.4 and B.5. Assign the times, positions (using D D , and D ), speeds (.$/, s , s and s ) and accelerations (zero or a ) corresponding to the four quintic control points that form the non-zero acceleration portion of the motion. 2  3  h  2  3  a  107  h  2  max  Appendix B  108  (Control Point) i  1  time (t )  position (p )  velocity (v„,)  acceleration (a )  0  0  Si  0  gi  1  a/  2 D, dtmax 3 tq2+(s -Sa)la x D,+D 4 D,+D +D tq3~^dtmax 5 t +{D-{D,+D +D ))ls D Table B . l . Quintic Control Point Conditions for the SAP Algorithm b  2  ma  2  (l4  2  3  Sa  &max  Sb  &max  s s  3  2  2  ai  2  0 0  Figure B. 1. Smooth Trajectory Generated with a Sequence of Quintics (• indicate the quintic control points)  B.2. Quintic End Conditions for a Trajectory Based on an Acceleration Pulse The quintic end conditions (time, position, velocity, and acceleration) for a trajectory between two waypoints based on an acceleration pulse are provided in Table B.2. The quintic control points are shown in Figure B.2 (taken from Chapter 3, cf. Figure 3.4). The Acceleration Pulse algorithm is repeated below for clarity. Acceleration Pulse Algorithm (s? > s^ > 0) 1.  l¥(ds<ds „&D<D ) Calculate dtfromEq. 3.24, and a from Eq. 3.25. IF (ds<a dt) Recalculate dtfromEq. 3.26 and a from Eq. 3.25. ELSE IF(£><£>„„•„) Calculate dtfromEq. 3.24 and a from Eq. 3.25. ELSE W(ds < ds „) Calculate dtfromEq. 3.26 and a from Eq. 3.25. Assign the time, position, speed and acceleration corresponding to the three quintic control points that form the acceleration pulse portion of the trajectory. mi  min  peak  peak  peak  peak  mi  peak  2.  ' Control point 5 is only necessary if a speed cruise is present (i.e. if D-(D|+D +D ) > 0) 2  3  Appendix B  109  (Control Point) i position (p ) velocity (v ) time (t„i) 1 0 0 Si 2 dt Sl+dpeakdt/2 D„i 3 2dt D ,+D s,+a dt 4 t + (D-p )lv D v Table B.2. Quintic Control Point Conditions for the Acceleration Pulse Algorithm 2  3  ai  q  q3  0  q3  q2  1.5  acceleration (a ) 0  peak  q3  1  0.5  ai  q3  2  2.5  3  3.5  gi  Qpeak  0 0  4  time [s]  Figure B.2. Speed Limited Trajectory (• indicate the quintic control points)  B.3. Algorithm followed when s > s t  2  When the speed of the first way-point is greater than that of the second way-point, the two speeds are interchanged and the trajectory is calculated following the algorithm outlined in Section 3.2. The quintic control points are then modified to reflect the actual way-point speeds, as shown in the algorithm below. This algorithm is also used to modify the control points for a ramp down from maximum path speed, s k, pea  to the speed at the end of the trajectory, s , when s k is larger than s (Section 3.2, Case B). 2  pea  Quintic control point 4 is only necessary for a speed limited case. D ^a, dt  3  2  q  „2  D  nax  =<W&  J_ 1_ 4  2  n  1  + s dt x  2  Appendix B  110  Algorithm Used to Modify the Control Points for a Deceleration Trajectory j = number of control points FOR \=\:j, 1. Adjust the control point time: tqi- tqj-tq(j-j + ])  2. Adjust the control point position:  Pqi = Pql-Pq(j-i+\) 3. Adjust the control point velocity:  4. Adjust the control point acceleration: Qqi  END  =  " i7(/'-i + l) a  Appendix C Algorithm Proof In this appendix it is shown that there is only one positive real root in dt (and two complex roots) for Eq. C. 1 (Eq. 3.24), \fs,>0,D>0 andj  max  > 0.  dr+- -dt L  r  :  Jmax  — = 0,  (C.l)  max  where D is the distance (positive, real, non-null, number), j  max  is the jerk limit (positive, real, non-null,  number) si is the speed at the first way-point (positive real number). It is important that the real root be positive since dt is a time that must be greater than zero. It has been shown that a cubic equation of the form ? + kt + lt+m = 0  (C.2)  2  has one real root and two complex roots if k, I, and m are real and U > V* [38], where 2  _ 2k -9kl + 27m 54 3  TT  k  2  -3/  V =-  - .  (C.4)  ForEq. C . l , Jfc =  0,  (C.5)  1= ^  ,  (C.6)  J max m=- ^ - , ^•J max  (C.7)  (k, /, and in are real) and therefore, U=-^-, 4/„  (C.8)  ITS  V = --^-.  (C.9)  111  Appendix C  112  If s, > 0, D > 0 and j  max  > 0, then L/  2  =  '  nD ^  \  4/J max J  nD ^  (  (CIO)  14/max J  and 3  /  7tS  V =  x  3  v  \  7TS\  (C.ll)  ^Jmax j  -'Jmax j  Therefore, U > V and there is only one real root in dt for Eq C. 1. 2  3  Furthermore, this real root is equal to (C.12) where L = -sign(U^\u\ + Ju  2  -V  3  3  ,  (C.13)  and (C.14) lo,(i=o)  Therefore, for the equation under consideration (Eq. C l ) , (C.15)  L= U + and since U > V , L > l\u\, and the real root, dt,: 2  3  dt = L + — A  (C.16)  If F=0, then L = (2|(7|) and l/3  dt =  TtD  A  (C.17)  max  In this case, dti > 0 since both D and j  max  are positive non-null numbers.  Otherwise V< 0, and dt, > 0 only if L  2  >-V.  (C.18)  Appendix C  11_3  This is in fact the case, since after intermediary calculations Eq C. 18 can be transformed into 0>2K |<L/|. 3/2  (C.19)  Since V < 0, Eq.C.19 holds and dt- > 0. Thus, the single real root of Eq C.l is always a positive real V2  number as long as si > 0, D > 0 and j  max  > 0.  Appendix D Quintic Control Points for Case B (iia) This material pertains to Section 3.2, case B, subcase iia. When the maximum speed, s  max  cannot be  reached due to distance limitations, it is necessary to shorten the acceleration cruises so that the distance, D, is respected. As defined in the section, D is the distance covered by a sustained acceleration pulse x  used to ramp up from s to s , and D is the distance covered by a sustained acceleration pulse used to s  ramp down from s  max  max  z  to s . Since D > Dn (Eq. 3.30), two sustained acceleration pulses will be used to 2  mil  move as fast as possible between the two way-points, however, in this case, D < D + D , and therefore, x  the maximum speed reached, s  will be lower than s :  peak  max  ^max^max peak  s  z  I  S  + S  i  +  + ^amaxD  =  a dt (s max  max  l  \  + s ). 2  (DA)  where s and s were found using the SAP algorithm for each motion. ax  az  The speeds s and s and the distances D, and D, remain the same as calculated using the SAP ax  az  x  z  algorithm, however, the speeds s/, and Sf, are changed as follows: x  z  a bx  S  ~ bz  ~ peak  S  S  dt  (D.2)  max max  The distances D , D , D and Dj are also modified: 2x  2z  3x  Z  D =-  bx  S  3x  D  =  max lax  a  D =22  Di, 3z =  ax  (D.3)  S  2amax  2x  dt  hz  S  v4" V j  +  f  bx max  S  df  ••  az  S  (D.5)  2a „  a„dtmax  \_  l +  max  (D.4)  bz max-  S  dt  (D.6)  This provides the information necessary to calculate the quintic control points for the motion of case 2(iia) when D < D +D . x  z  114  Appendix E Blend Acceleration and Jerk Limits It was stated in Chapter 4 that the maximum acceleration and jerk attained during a blend are s  l\ + cosO  2  blendMax  a  ~  V  ~~T,  p • tightness V  Z  2  '  (E.  \5s JblendMax  where,  5  —  _  .  ,  C 2  ztightness  O  S  \  1)  (E.2)  ~  \ 2j  is the speed, p is a proportionality constant, tightness indicates how close to the point the path  must pass, and 6 is as shown in Figure E. 1. The derivation of these two equations is provided in this Appendix. Figure 4.3 is reproduced here for reference purposes (Figure E.l). This appendix assumes that the blend under consideration is a geometric blend as outlined in Section 4.2.  270  210 x coordinate [mm]  Figure E. 1. 2D Blend Geometry  115  Appendix E  U6  E . l . Constant of Proportionality In this section it will be shown that there is a constant relationship between the maximum acceleration during a geometric blend and the constant absolute acceleration required to transit between the initial and final blend velocities. If the change in velocities were done linearly, the corresponding acceleration would be constant, a  ,. Establishing the relationship between a  comlan  ObiendMax,  , and the maximum blend acceleration,  cnnskm  is of interest since the constant acceleration equations of motion could then be used to determine  a relationship between the maximum permitted acceleration, a , and the change in velocity during the max  blend. It will be shown below that the magnitude of a  is directly proportional to the magnitude of  cnnsmm  dbiendMax  through the proportionality constant, p: constant  a  =  P  ' hlendMax  (E-3)  a  The equation for a blend between two straight lines xi and x , is reproduced here from Chapter 4 (Eq. 2  4.7),  x{o) = xX(j)+a{G){x {o)-xX(j))-id3((j)v , 2  (E.4)  d  where in the geometric blend case,  x (&) = b + m a, x (a) = b + m ]  ]  ]  2  2  2 CT,  (E.5) (E.6)  a{o) = 6cr -15cr +10(7 , s  4  (E.7)  3  ^(CT) = CT -3CT +3CT -CT , 6  5  4  (E.8)  3  v, = / w - / « , ,  (E.9)  2  *.=/»>„>  b =p -2Tv ,  /M,=2TV,„,  m =2Tv ,  2  2  ]h  (E.IO)  lh  (EM)  ]h  and 2-ris the blend time. Initially, the location and magnitude of the maximum blend acceleration must be determined. This requires developing the equation describing the jerk in the c^domain. Taking the first, second and third derivatives of x(d) with respect to o~, leads to the following equations (from which the null terms have been removed): v(cr) = x(o) = *, (<J)+a{ci){x2 (cr) a{a)  = x{<y) = 2a{(j){x  2  {CJ)-  {a))+d{cj){x  2  i,  (cr))+a{a){x  2  {CJ)-X  {<r)-x  I  x  (cr)) -  (CT)) -  K${rj)  K${a)v , d  Appendix E  j(o) =  117  X{<J) =  - i , (tr))+a(tfX* 2  3d{(j\x (<T) 2  M - *, M ) - * 5 & ' M v „  (E.14)  Substituting Eqs. E.5-E.9 and their derivatives into Eq. E.14 j(a)= 60(b -b )+45(HI -m ) 2  x  2  x  + o[- 360(* - A,) - 300(i» -m,)]+ 2  2  cr [360(6 -6,)+ 450(JII - « , ) ] - 180cr (m -m,) 2  3  2  (E.15)  2  2  Therefore, the local acceleration maxima and minima are located at : 1  1  2  6  K  (E.16)  I_l 2  (E.17)  6  4(A -6, 2  0,  )+3(m -m ) 2  ]  (E.18)  2{m -m ) 2  x  and the two extremities cr= 0, a= 1. These locations are plotted on Figure E.2.  Figure E.2. Plot of Acceleration Profile for the 2D Blend of Figure E.l. Solving for the numerical value of a requires defining b2-b\ and m -mi in terms of the blend positions 3  and velocities. Given that a geometric blend is being considered,  2  Appendix E  118  Pu,-2Pi  + Pu,^  r v  «,-v  =s  l a  (E.19)  tightness  and from Eq. 4.19, T=  tightness  (E.20)  then _ (Pu,- Pi =  +  2  v  —v  P j  (E.21)  Thus, from Eqs. E. 10, E. 11 and E.21 , 2~^  b  = Pi„ ~ 27 v,, - / > , „ = - p + 2p-p , lh  (E.22)  la  « - i w , = 2 r ( v - v , J = 2 ( / » - 2 p + /» ). 2  IA  14  l  (E.23)  lfl  Substitution of Eqs E.22 and E.23 into Eq. E.18, gives 0} = 0.5. Thus, provided that all the conditions stipulated for a geometric blend are met, the local acceleration maxima and minima always occur at the same path location and corresponding time location in every dimension (as can be seen in Figure E.2). Substituting the f i v e a values corresponding to the local maxima and minima locations into the acceleration equation (E.13) leads to the equations corresponding to the values of the local maxima and minima : a(o-, )= 5.774(6 -b ) + 4.13l(m -m,) = 2.500(/> -2 + p ),  (E.24)  ) = -5.774(6 - 6,) - 1.637(m - m, )= 2.500(/> - 2p, + p ),  (E.25)  2  a(<7  2  {  2  2  1A  2  Pl  lft  Wl  Xa  a(<Tj)=0.938(« -jfi )=1.876(p -2/» + /» ), 2  1  16  l  lfl  a(0) = a(l) = 0.  (E.26) (E.27)  The absolute acceleration profile has two equal local maxima and one local minimum (assuming a positive acceleration profile). The acceleration values of interest are those corresponding to a(0\) and a(ch), since these are the maximum absolute accelerations occurring during a blend (Figure E.2). That is ^ w ^ * =2.500(/> -2/> + p ). 16  1  ]a  The maximum path blend acceleration is obtained by taking the magnitude of Eq. E.28. The constant acceleration vector, when accelerating linearly from nt/ to nt2 is:  (E.28)  Appendix E  119  (E.29)  1-0  Therefore, from Eqs. E.3, E.23, E.28, and E.29, the relationship between the constant acceleration and the maximum blend acceleration vectors is .  2.000fo - 2 6  constant  P ] +  P ] a  )  - °0(Plft ~ P\ + P\a  2 5  blendMax  a  = 0.800.  2  (E.30)  )  That is, each element of awendMax is proportional to its corresponding element in a  conslanl  by p.  E.2. Derivation of the Relationship Between the Maximum Acceleration and the Speed for a Geometric Blend Based on a constant acceleration motion between v, and v/i, and dividing the acceleration into x and y a  components as shown in Figure E.3, (E.31)  Itightness and s sin 6 2  (E.32)  a., - • 2tightness Then, the magnitude of the constant acceleration is : \ + cos6  (E.33)  tightness tightness  \ax  V  v  Ulv  =S-COSO  = s-sind  \a  =- \aJ+ layJ  V  V  V  Vlbx = S  =  V, J  Figure E.3. Figure Illustrating the Variables Used in Equations E.31 and E.32  \= S  Appendix E  120  Thus, using the result from Section E . l , (Eq. E.30) and Eq. E.33 the magnitude of the maximum blend acceleration is defined as: s . ,,  \\ + cos0  2  1  blendMax  ~  „  t  \\  ~  p • tightness V  •  (E.34)  2  To ensure that the magnitude of the maximum blend acceleration is no larger than that allowed {a \ it is max  possible to limit the incoming/outgoing speed of the blend, i.e., ,<  \P ^htness a  T  '  \\ + cosd  2 so that the maximum allowable acceleration, a , is respected during a geometric blend. max  E.3. Derivation of the Relationship Between the Maximum Jerk and the Speed for a Geometric Blend The relationship between the maximum jerk and the incoming and outgoing speed of the blend can be derived much in the same manner as in Section E.2. In this case, to find the location of the local maximum we must set the derivative of jerk with respect to the parameter o, namely the snap, %, to zero and solve for the corresponding roots : do  do  do  Substituting Eqs. E.5-E.9 and their derivatives into Eq. E.36, x{o) = -360(6 - b )-300(/« 2  x  2  w,) + <r[720(6 - h ) + 900(m - m,)]- 540<7 ( m - m,). (E.37) 2  2  x  2  2  The roots of Eq. E.37 are 12(^ -^ )+15(m -m,)±Vl6fe-3,) +16(ft -5,Xm -iii )+5(w -iii ) 2  2  A  1  2  2  2  =  l  2  a  1  .  (E.38)  18(/M -m, J 2  When Eqs. E.22 and E.23 are substituted into Eq. E.38, it can be shown that <T,=|,<T =|. 2  (E.39)  Appendix E  121  There are four locations at which the jerk may be maximum : a,, a and the blend extremities (i.e. a= 0 2  and o"= 1). From Eq. E.15, E.22 and E.23, the value of the jerk at these locations can be found to be : j{0)= 30(p -2/», + p ),  rn  (E.40)  ia  I6  10/ = -—\Pib-  * P\ + P\ ),  2  (E.41)  a  10  (E.42)  ,3, and j{l)=-30(p -2 lb  Pi  +  ).  Pla  (E.43)  These locations are plotted on Figure E.4.  — x coordinate y coordinate o local maxima/ minima  Figure E.4. Jerk Profiles for the Blend of Figure E. 1 The maximum absolute jerks always occurs at the extremities, that is, at the start and the end of the blend (Figure E.4). To find the relationship between the speed and the magnitude of the jerk in the time domain, it is necessary to convert the jerk and velocity from the cr-domain to the time domain using the identity  At)-  8r  (E.44)  3  and from Eq.23 m ,-m, = 2 r ( v , - v , J , i  (E.45)  Appendix E  122  giving  ik±°Wi<^)=l .  (E.46)  5  The magnitude of the maximum blend jerk is then j blendMax  ~  "•  . 7  (E.47)  4r However, from Eqs. E.20 and E.21: 15s J  blendMax  p -2 w  P l  + p  la  (E.48)  ^tightness  3  Given that 2/>,+ /»„ =  P t t - P i  +  =2\\tightness  P t a - P t  -  P l f -  Pu  2  (E.49)  and (Q\  -^tightness* -  cosl \ 2J  Pu,~ Pu  (E.50)  tightness  (Figure E.5), it can be shown that 15^ cosl J  blendMax  \2j  (E.51)  Itightness  Therefore,  2  s<  • Jmax  • tightness  (E.52)  15cos\  .2  so that the maximum jerk during the blend is no more than the maximum allowable jerk, j . max  One can note that either Eqs. E.48 or Eq. E.51 may be used to limit the jerk. Use of Eq. E.48 is preferred since it does not require the computation of an inverse cosine.  Figure E.5. Diagram Indicating the Geometry Used to Derive Eq. E.51  Appendix F Estimation of Task Space Kinematic Limits and Calculation of Joint Space Path Limits The problem of determining reliable joint and Cartesian space kinematic limits has not been thoroughly treated in literature. Kinematic limits (as derived from worst case dynamic limitations) are highly configuration dependent. Most authors develop their work based on the assumption that such limits can be found or estimated, but do not provide details. The manipulator kinematic limits derived using the methods described below are simply estimates. They will not ensure that the manipulator dynamic limits are respected due to the high configuration dependence. However, the estimates are conservative enough that the manipulator should generally be within its physical capacity, while not being so conservative that the manipulator's capacity is underused. F . l . Joint Space Kinematic Limits The joint space kinematic limits necessary for the trajectory generation algorithm consist of the maximum path speed, acceleration and jerk. Actuator torque and speed limits can be obtained from manufacturer data sheets. The manufacturer speed limit provides the actuator speed limit, while the joint actuator acceleration limit can be derived through use of the manipulator Lagrangian dynamic equation: M{y)y+y C(y)y+G(y)=T.  (F.l)  T  where ye R" is the vector of joint positions (n corresponds to the number of joints), Te R" is the vector of actuator torques, M(y)e R" " is the inertia matrix of the manipulator, C(y)e R"*" x  xn  is a third order  tensor representing the coefficients of the centrifugal and coriolis forces, and G(y)e R"is the vector of gravity terms. To determine the joint space acceleration limit, y , the actuator torque and speed limit are used in max  y = M{yY [T- y C{y)y- G{y)\ T  124  (F.2)  Appendix F  125  and the joint angles are incremented. The minimum acceleration for each joint is used to determine the maximum allowable acceleration vector. Alternatively, an optimization technique can be used to determine the limits. Generally, the actuator torque rate limits are not provided by the manufacturer. In fact, the choice of the jerk limit is a trade-off between the smoothness of the motion and the motion time. As the jerk limit is increased, the trajectory starts to resemble an LSPB trajectory causing poorer tracking. However, too low a jerk limit compromises the speed of the motion without providing much increase in tracking accuracy. Therefore, the joint jerk limits are set based on an estimate of the time it should take to ramp up to the maximum joint acceleration, At , Jerk  Y  m  a  x  = i f ^ -  (F.3)  These manipulator-specific limits are the individual joint limits, and must only be determined once. The path limits are computed as described in Section F. 1.1. F.l.l. Joint Space Path Limits Herein, the manipulator trajectory is generated based on joint space straight-line paths joined by blends (Chapter 5). The trajectory generation algorithm requires one-dimensional kinematic limits along the joint space path since the trajectory is initially designed in one dimension (Chapter 3). However, for industrial systems, the joint space limits are given in terms of the individual joints, rather than as a single number applicable to the entire path, as is commonly done for the task space limits. The particular joint that imposes the limit will differ depending on the straight line or blend motion. Thus, the one-dimensional kinematic limits must be generated for each trajectory segment. This is done through projection of the individual joint limits into the space of the trajectory path, as explained below. It should be noted that the speed, acceleration and jerk along the path may be limited by different joints. a. Projection for Straight Line Segments The projection factor for joint / between two way-points p and p is 0  \pM)-pM  Proj=  t  (F4)  \P\~ Po\ The speed limits at the start and the end of the straight line, the maximum speed limit, the acceleration limit and the jerk limit for joint / are obtained by dividing the nominal joint limit by the projection factor (Figure F.l):  Appendix F  126  _ . „. . . Kinematic Limit/ Path Kinematic Limit,. = '-. Proj, T  (F.5)  The path joint limits are the minima of the limits obtained for all the joints, namely: Path Joint Limit = m\n(Path Kinematic Limit),  (F.6)  where min is a function that chooses the minimum Path Kinematic Limit.  r*  Kinematic limiti  •  Figure F . l . Kinematic Limit Projection for a 3D Straight Line Path b. Projection for Blend Segments The geometric blend segments only require computation of acceleration and jerk limits since the maximum speed is fixed by the straight lines joined by the blend (the maximum speed in a blend occurs at its end points). These limits are computed in a similar manner to that described in (a), except that in this case pi is the point at the end of the blend and o is the point that is being blended around. P  F.2. Task Space Path Kinematic Limits The task space kinematic limits necessary for the trajectory generation algorithm consist of the maximum Cartesian path speed, acceleration and jerk, and the maximum rotational path speed, acceleration and jerk limits. These limits are computed from the joint space limits based on differential kinematics. The task space speed is related to the joint space speed through v=j(y)y,  (F.7)  where is j(y)s R "the Jacobian matrix. Through differentiation with respect to time, it can be shown nx  that the task space acceleration is related to the joint space speed and acceleration through  Appendix F  127 a = j(y)y+j(y)y.  (F.8)  The limits are determined through iteration of Eqs. F.7 and F.8 based on varying the manipulator configuration while y = y  max  and y = y . The Cartesian space speed and acceleration limits are chosen max  as the minimum of the average speeds and accelerations in the x, y and z directions, respectively (since the motion could be along any of the axes). The orientation limits are taken to be the minimum of the kinematic limits of joints 4, 5 and 6. The maximum jerk limit is set based on the estimated time it should take to ramp up to the maximum task space acceleration, a , At , max  /erk  J, max  amax  (F.9)  Jerk  As opposed to the joint space limits, the task space limits are only determined once (i.e. no projection onto the path is used), as is common in industrial trajectory generation routines.  Appendix G Quaternions G.l. Rotation Matrices and Quaternions The change in end effector orientation between two way-points (e.g. p and p ) can be represented in the 0  t  form of a rotation matrix: R = R'R , =  (G.l)  where R o and R , are the rotation matrices corresponding to the orientations with respect to a world w  w  frame, F , at way-points p and />/ respectively. Every rotation can also be expressed using an axis-angle n  w  formulation [42]. Thus, to move smoothly about a single axis (i.e. not "wobbling") between the end effector orientations at p and p , the end effector frame must rotate about the axis of the rotation matrix, 0  t  Roi-  1  f  (G.2)  2sintp  2 r  2l  \2  r  by an angle, (G.3)  <p = cos' 2  This can also be represented in quaternion form as:  I2J  cos\  f sin x  12  Z= fy  S l n  fz  s i n  \  \  2  (GA)  J  z.  v2y  Z is a unit quaternion because the sum of the squares of its four components is equal to 1. To transform a unit quaternion into a rotation matrix, the following equation is used [42]: 128  Appendix G  129  f, (l - c )+ c fj (l -c )- f s^ fj (l - )+ f s 2  h  02  R=  f "%  y  f  fjy  )+ f  z  S  h  .fJ ( -C )~fy^ [  Z  where  h  2  2  h  2  = cos(</> ) and  h  (l " )+ fyfM~C )+f S  1  x  z  ff y  2  z  y  Cfc  (l - C  h  )-  h  fS x  h  (G. 5)  //(l-C^J+C^  h  = sin(^?), and from Eq. G.4, (G. 6)  </> = 2co5~'(Z ,), 2  H  and  sin2 J (G. 7) 5m  5m  A computationally efficient manner of calculating Eq. G.5 is as follows: f ^ = /-/ +  5(0 )-  T  C O  2  \  "l  0  o"  0 1 0 -f-f 0 0 1  + sin{</> )-S{f), 2  (G.8)  )  where/ is the transpose of vector/and S(f) is the skew symmetric matrix corresponding t o / T  G.2. Quaternion Multiplication and Transpose A quaternion can also be described as: Z = Z + Z i+ZJ w  x  + Z k, 2  (G.9)  where i = j =k = -1 and ij = k, ji =—k [51, p.62]. Thus, the multiplication of two unit quaternions H and Z can be calculated as: HZ=[Z]H,  (G. 10)  where  z z  -z z  Zy  ~Z  w  x  z,  -z z  x  y  w  z  Z  Z  w  -z -z  z  y  Z  x  z., - z . z...  (G. 11)  Appendix G  130  The transpose of a unit quaternion is simply: Zw (G. 12)  G.3. Algorithm Used to Generate a Rotation Matrix at Each Sampling Interval As described in Chapter 5, a quintic trajectory, <p(t), that starts at 0 and ends at fa, smoothly interpolates between the orientations p  0  and /?/. From this, the rotation matrix describing the orientation of each  sampling instant point with respect to the world frame is generated for use in the inverse kinematics. The algorithm used to generate the rotation matrix with respect to the world frame at every sampling instant is provided below. It should be noted that the manipulations could also have been done using an axis-angle representation rotation matrix instead of the quaternion form.  Algorithm To Generate a Rotation Matrix at Each Sampling Interval 1.  Compute ZKi> and Zwh which correspond respectively to the rotation matrices of p and pi (Eq. G.4).  2.  Compute Z , the quaternion corresponding to the change in orientation between p and p, (Eqs. G.10 and  0  w;  0  G.12): (G.13) 3.  Calculate the normalization constant:  N const 4.  (G.14)  FOR each quintic trajectory point, $(/), a. Calculate the first quaternion element, 7  Z =cos w  -t  1  (G.15)  b. IF(1-Z„ >0) 2  (G.16) ELSE K= 1 END  (G.l 7)  Appendix G  131  c. Compute the quaternion corresponding to a rotation of <j)(i) about axis/with respect to the frame ofp : 0  z z 01, K (G.l 8)  ^01 y  K K d. Compute the quaternion representing the rotation of /», with respect to the world reference frame (Eq. G.10) Z , = Z^Z . m  (G.19)  m  e. Compute the corresponding rotation matrix, R (Eq. G.5)). Ki  END  Appendix H Further Experimental Results H . l . Stop Point to Stop Point Task Space Motions  H.l.l. Task Space Experiment 1 Point  x [mm]  y [mm[  z [mm]  yaw [rad]  pitch [rad]  roll [rad]  1  510  355  310  0.165  0.0731  -0.0113  2 555 -360 240 0.165 0.0731 -0.0113 Table H. 1. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 1 Industrial Planner Demanded Speed Along Path [mm/s (% of M M A S ) ]  Quintic Planner Motion Time [s]  Path Deviation R M S [mm]  0.5 0.5 0.5 0.5 0.5 0.5 0.6  102  (10%)  203  (20%)  305  (30%)  406  (40%)  508  (50%)  7.9 3.9 2.7 2.0 1.6  610  (60%)  \A  711  (70%)  1.2  1  j  7.2 3.7 2.6 2.0 1.7 1.5 14  1.2 1.3 914 (90%) 1.1 1.3 1016 (100%) 1.1 1.2 Table H.2. Results for Task Space Experiment 1 813  (80%)  0.6 0.6 0.6  0.1 0.2 0.2  9H9KI 0.3 0.3  111011111 4  Maximum Path Deviation [mm]  1.5 1.5 1.5 1.5 1.5 1.6 1.6 1.5 1.5 1.5  0.5 0.5  SSillll 0.8  Willi UN  •  The tool transform used in this case was 10.5 mm.  •  At comparable motion times, the quintic planner provides a 38% to 47% improvement in maximum path deviation, and a 33% to 40% improvement in average path deviation.  •  At demanded speeds below 406 mm/s (40% of the Manufacturer Maximum Allowable Speed (MMAS)), implementation of the quintic planner trajectories provides a faster motion and better path deviations as compared to the industrial planner trajectories.  •  At demanded speeds above 406 mm/s (40% MMAS), the industrial planner violates the acceleration limit. 132  Appendix H  133  H.l.2. Task Space Experiment 2  Point  x [mm]  y [mm]  z [mm]  yaw [rad]  pitch [rad]  roll [rad]  1  430  355  430  In ~9~  n 360  n 90  In  n n ~9~ 360 90 Table H.3. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 2 2  430  265  400  Industrial Planner Demanded Speed Along Path [mm/s (% of MMAS)1  Quintic Planner Motion Time [s]  Path Deviation R M S [mm]  Maximum Path Deviation [mm]  102  (10%)  1.1  1.0  OJ  0.2  0.3  0.4  203  (20%)  0.6  0.2  0.2  0.4  305  (30%)  406  (40%)  0.6 0.4 0.3  0.5 0.5  0.3 0.4  0.5 0.5 0.5  508  (50%)  610  (60%)  711  (70%)  813  (80%)  914  (90%)  0.3 0.2 0.2 0.2 0.2  0.5 0.5 0.5 0.5 0.5  0.5 0.6 1.2 1.5 1.5  0.2 1016 (100%) 0.5 Table H.4. Results for Task Space Experiment 2  1.6  03 03 0.3 0.3 0.3  0.6 0.7 0.8 1.1 2.4 2.9  3.0 3.2  0.5 0.5 0.5 0.5 0.5 0.5  •  The tool transform used in this case was 0 mm.  •  At a demanded speed of 203 mm/s (20% MMAS), the quintic planner trajectories provided 20% poorer maximum path deviation, while providing equivalent, average path deviation. No other comparisons are possible since no other times are equivalent.  •  It is shown that the quintic planner trajectories limit the motion times, and as a consequence, the maximum and average path deviations during the motion. The large increases in maximum and average path deviations with an increase in demanded speed, obtained with the industrial planner, are a result of violated kinematic constraints.  •  At demanded speeds above 203 mm/s (20% MMAS), the industrial planner violates the acceleration limit.  Appendix H  134  H.l.3. Task Space Experiment 3  Point  x [mm]  y [mm]  z [mm]  1  -355  -450  280  yaw [rad]  pitch [rad]  roll [rad]  lbr 36  0  5rr 18  5/r lbr 0 18 36 Table H.5. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 3 2  365  -500  410  Industrial Planner Demanded Speed Along Path [mm/s (% of MMAS)1 102 (10%)  Quintic Planner Motion Time [s] 8.0  7.3  Path Deviation RMS [mm] 0.4 0.2  203 (20%) 305 (30%) 406 (40%) 508 (50%)  4.0 2.7 2.1  3.8 2.6 2.1  0.4 0.5 0.6  0.4 0.4 0.5  Maximum Path Deviation [mm] 0.9 0.9 0.9 1.1 1.4 0.9 0.9 1.5  1.7  1.7  0.6  0.6  1.6  1.0  610(60%) 711 (70%) 813 (80%)  1.4 1.2  1.5  0.7 0.8  0.6 0.6  1.8 1.8  1.1  1.3  0.8  0.7  2.0  1.1 1.3 1.2  914 (90%) 1.1 1.3 1016 (100%) 1.0 1.2 Table H.6. Results for Task Space Experiment 3  0.8 0.9  0.7 0.7  1.8 1.9  1.3 1.4  •  The tool transform used in this case was 105 mm.  •  At comparable motion times, the quintic planner provides a 22% to 40% improvement in maximum path deviation, and a 13% to 17% improvement in average path deviation.  •  At demanded speeds below 406 mm/s (40% MMAS), implementation of the quintic planner trajectories provides a faster motion and better path deviations as compared to the industrial planner trajectories.  •  At demanded speeds above 508 rnm/s (50% MMAS), the industrial planner violates the acceleration limit.  Appendix H  135  H.1.4. Task Space Experiment 4  Point  x [mm]  y [mm]  z [mm]  yaw frad]  1  -355  -450  280  0  pitch [rad]  roll [rad]  71  An ~6  7 71  An ~6~ 7 Table H.7. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 4 2  365  -380  410  0  Quintic Planner  Industrial Planner Demanded Speed Along Path [mm/s (% of M M A S ) ]  Motion Time [s[  Path Deviation R M S [mm]  Maximum Path Deviation [mm]  102  (10%)  8.0  7.3  0.4  0.2  1.1  1.1  203  (20%)  3.8  0.4  (30%) (40%)  2.0  0.5 0.5  1.2 1.3  406  2.6 2.0  0.3 0.4 0.5  t.l  305  4.0 2.7  1.3  1.1 .0  508  (50%)  610  (60%)  1.7 1.4  0.6 0.7  0.5 0.6  1.5 1.7  1.1 1.1  711  (70%)  1.2  1.7 1.5 1.4  0.7  0.6  1.6  1.1  813  (80%)  1.1  1.3  0.8  0.6  1.7  I.I)  1.0 1.2 1.0 1.2 1016 (100%) Table H.8. Results for Task Space Experiment 4  0.9  0.7 0.7  1.9 1.7  1.2  914  (90%)  0.9  •  The tool transform used in this case was 105 mm.  •  At comparable motion times, the quintic planner provides a 23% to 35% improvement in maximum path deviation, and a 14% to 17% improvement in average path deviation.  •  At demanded speeds below 406 mm/s (40%) MMAS), implementation of the quintic planner trajectories provides a faster motion and better path deviations as compared to the industrial planner trajectories.  •  At demanded speeds above 406 mm/s (40%) MMAS), the industrial planner violates the acceleration limit.  Appendix H  136  H.1.5. Task Space Experiment 5  Point  x [mm]  y [mm]  z [mm]  yaw [rad]  pitch [rad]  roll [rad]  1  520  320  660  -0.0716  0.0113  0  2  410  455  210  -0.0716 0.0113 0 Table H.9. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 5 Industrial Planner Demanded Speed Along Path [mm/s (% of M M A S ) ]  I Quintic Planner Motion Time |s|  Path Deviation R M S [mm]  Maximum Path Deviation [mm]  4.9 25 1.8 1.4 1.2 1.1  0.2 0.3 0.3 0.3 0.4 0.5  0.6 0.7 0.7 0.7 0.8 0.9  0.8 1.0 0.8 813 (80%) 914 (90%) 0.7 1016 (100%) 0.7 Table H. 10. Results for Task Space Experiment 5  0.5 0.6 0.6 0.7  102  (10%)  203  (20%)  305  (30%)  406  (40%)  508  (50%)  610  (60%)  711  5.3 2.7 1.8 1.4 1.1 0.9  (70%)  HKHI  0.2 0.2 0.3 0.3 0.3  IHMB 0.4 0.4 04 0.4  1.0 1.1 1.1 1.3  0.4 0.5 0.5 0.6 !>.-  0.6 0.7 0.6 0.7 0.7  •  The tool transform used in this case was 105 mm.  •  At comparable motion times, the quintic planner provides a 14% to 29% improvement in maximum path deviation, and equivalent average path deviation.  •  At demanded speeds below 305 mm/s (30% MMAS), implementation of the quintic planner trajectories provides a faster motion and better path deviations as compared to the industrial planner trajectories.  •  At demanded speeds above 406mm/s (40% MMAS), the industrial planner violates the acceleration limit.  Appendix H  137  H.1.6. Task Space Experiment 6  Point  x [mm]  y [mm]  z [mm]  yaw [rad]  pitch [rad]  roll [rad]  1  410  455  210  -0.0716  0.0113  0  2  520  320  660  -0.0716  0.0113  0  Table H. 11. Coordinates of the Straight Line Path Start and End Points for Task Space Experiment 6 Industrial Planner Demanded Speed Along Path [mm/s (% of MMAS)1  Quintic Planner Motion Time [s]  Path Deviation R M S [mm]  102 (10%) 203 (20%) 305 (30%) 406 (40%) 508 (50%) 610 (60%) 711 (70%) 813 (80%) 914 (90%) 1016 (100%)  5.3 4.9 2.7 2.5 1.8 1.8 1.4 1.4 1.1 1.2 0.9 1.1 1.0 0.8 0.8 1.0 1.0 0.7 0.7 1.0 Table H. 12. Results for Task Space Experiment 6  0.2 0.3 0.3 0.4 0.4 0.5 0.5 0.6 0.9 1.5  0.2 0.3 0.3 0.4 0.4 0.5  lllliB 0.5 0.5 0.5  Maximum Path Deviation [mm]  0.5 0.6 0.6 0.7 0.9 1.0 1.1 1.4 2.6 4.7  0.8  ns n.8 (i.8 0.8 0.8  o.s 0.8 0.8 (l.s  •  The tool transform used in this case was 105 mm.  •  At demanded speeds of 305 mm/s (30% MMAS) and 406 mm/s (40% MMAS), the quintic planner provides a 13% to 25% poorer maximum path deviation, and equivalent average path deviation.  •  For a motion time of 1.1 s, the quintic planner provides an improvement of 11% in maximum path deviation and a 20% poorer average path deviation.  •  At demanded speeds below 305 mm/s (30% MMAS), the implementation of the quintic planner trajectories provides a faster motion, equivalent average path deviation and poorer maximum path deviation.  •  As the demanded speed increases, there is a large increase in maximum path deviation for the industrial planner trajectories.  •  At demanded speeds above 305 mm/s (30% MMAS), the industrial planner violates the acceleration limit.  Appendix H  138  H.1.6.1 Explanation of Results for Task Space Experiment 6  The maximum path deviation remains constant with increasing demanded speed for the quintic planner. It occurs during or shortly after the initial change in speed (during the first acceleration phase). For the industrial planner trajectories, at high demanded speeds, the acceleration limit is violated, resulting in large maximum path deviations, which occur during the acceleration phase. However, at demanded speeds below 305 mm/s (30% of MMAS), the industrial planner trajectories do not cause such large path deviations since the maximum acceleration is lower than the limit. This would indicate that the acceleration limit is too high for this manipulator path. This path has end-points identical to the path end-points of Section H.1.5; they are simply interchanged. The maximum path deviation for the path of Section H.1.5 occurred near the end of the trajectory, while the maximum path deviation for the path of Section H. 1.6 occurred near the beginning of the trajectory. This indicates that the given combination of trajectory manipulator configuration, speed and acceleration at this point may violate the joint limits.  Appendix H  139  H.2. Multiple Way-Point Task Space Motions  H.2.1. Horizontal Rectangular Motion Point  xfmml  y [mml  z[mml  1  315  -300  390  2  470  -300  390  3  470  300  390  4  315  300  390  5  315  -300  390  yaw [radl  pitch |rad]  roll [rad]  5n  71  \\7t  9 5n  2 7T  18 lbr  9 5n  ~i K  18 lbr  ~~9 5n  ~2 7t  18 \\7l  ~9~ 5n  2 7t  18 lbr  9 ~2 18 Table H.13. Coordinates of Way-Points for Task Space Horizontal Rectangular Path Industrial Planner Demanded Speed Along Path [mm/s (% of M M A S ) ]  Quintic Planner Motion Time [s]  (70%)  15.12 8.00 5.81 4.75 4.17 3.90 3.76  14.96 7.57 5.13 4.13 3.66 3.38 3.20  (80%)  3.74  3.09  102  (10%)  203  (20%)  305  (30%)  406  (40%)  508  (50%)  610  (60%)  711 813  3.80  3.02 2 1016 (100%) 3.90 Table H.14. Results for Horizontal Rectangular Path 914  (90%)  The tool transform used for this motion was 105 mm. At all demanded speeds, the quintic planner provides a faster motion than the industrial planner does.  Appendix H  140  H.2.2. Vertical Rectangular Motion Point 1 2 3 4  y [mm] zfmml yaw H pitch [°] roll [°] 510 115 240 0 0 0 510 465 240 0 0 0 510 465 575 0 0 0 510 115 575 0 0 0 5 510 115 240 0 0 0 Table H.15. Coordinates of Way-Points for Task Space Vertical Rectangular Path x [mm]  _  Demanded speed:  1 0 2  (10%) * way-point  305 (30%)  711 (70%)  600  600  500  500  1"  1016 mm/s (100%) (of MMAS)  UF  E,  E,  »400  «T400  x  CD  x ro  N  N  300  20( 100  300  200  300 y axis [mm]  400  20C 100  500  200  (a) Industrial Planner  300 y axis [mm]  400  500  (b) Quintic Planner  Figure H. 1. Comparison of Paths at Different Speeds for Vertical Rectangular Path 102 (10%)  Demanded speed:  305 (30%)  1000  1000  800  800  600  E 600 E,  !!  ij  1016 mm/s (100%) (of MMAS)  P  llii I ih ii  "8  CD CD Q .  711 (70%)  £ 400 H (/)  '  200 0 0  200 5  time [s]  10  (a) Industrial Planner  15  °0  5  time [s]  10  (b) Quintic Planner  Figure H.2. Comparison of Speed Profiles for Task Space Vertical Rectangular Path  15  Appendix H  141  Industrial Planner Demanded Speed Along Path [mm/s (% of MMAS)1 102(10%) 203 (20%) 305 (30%) 406 (40%) 508 (50%)  Quintic Planner Motion Time [s] 13.74 7.08 4.96 4.00 3.48  1.3.58 6.8S 4.67 3.78 3.38  610 (60%) 3.42 3.1ft 711 (70%) 3.55 3.05 3.01 813 (80%) 3.73 914 (90%) 3.95 3.01 1016 (100%) 4.17 3.01 Table H. 16. Results for Task Space Vertical Rectangular Path The tool transform used for this motion was 105 mm. At all demanded speeds, the quintic planner provides a faster motion than the industrial planner does. At demanded speeds above 711 mm/s (70% MMAS), the motion is limited by distance for the quintic planner. At demanded speeds above 610 mm/s (60% MMAS), the industrial planner motion time increases with increasing demanded speed, since the maximum path acceleration is reduced. This occurs when one of the path segments between two way-points becomes a pure blending motion (i.e. it is formed of two blend halves).  Appendix H  142  H.3. Stop Point to Stop Point Joint Space Motions  H.3.1. Joint Space Experiment 1  Point Joint 1 [rad] Joint 2 [rad] 1  l7Z  18 An  Joint 3 [rad[ Joint 4 [rad]  n  n ~6  13* 18  ~~9  Joint 5 [rad]  Joint 6 [rad]  In ~9~  5n  18  11* K n In 9 9 18 ~~6 18 ~7 Table H. 17. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 2  5n  Industrial Planner Demanded Speed Along Path [mm/s (% of MMAS)] 102 (10%) 203 (20%) 305 (30%) 406 (40%) 508 (50%) 610 (60%)  Quintic Planner Motion Time [s] 8.5 4.4  8.4  4.3  3.1  2.9  2.4  2.2  2.0  1.8  1.8  1.6  Dev iation of J oint [IO rad] 3  1  2  1  0  0 0 0 0 0  111  1.4 1.6 0 711 (70%) 813 (80%) 1.5 1.3 0 1.4 1.2 0 914 (90%) 1.4 1.1 0 1016 (100%) Table H. 18. Results for Joint Space ixperiment  Hi .»  ill 0 0  •  0 0 1 1 1 1 1 1 1 1  4  3 1  I1PHS  1 1  I  1 2 2 2 1 i  0 (1  1 a&' -  1 •1 1 1 1 1 1  i i i i i i i  5  0 1 2 2 3  0  3  4  3 3  4  4  4  4  4  1  3 3 3  4  0 1 2 2 3 3 3 3 3 3  6  4  1 1 1 1 1 1 1 1  4  1  1 1 1 1 1  4  1  1  1 2 3. j 3 4  1 |  1 ]  The quintic planner provides a faster motion at all demanded speeds, and equivalent accuracy at the end points.  Appendix H  143  H.3.2. Joint Space Experiment 2  Point Joint 1 [rad] 1  Joint 2 [rad]  K ~~9  Joint 3 [rad]  Joint 4 [rad]  Mn  0  71  ~ T  18  Joint 5 [rad]  Joint 6 [rad]  n  0  ~ ~9  5K 33;r n n n 36 18 36 18 12 18 Table H. 19. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 2  5n  2  Industrial Planner  Quintic Planner  Demanded Speed Along Motion Path Time [s] 1 [mm/s ( % of MMAS)] o III! 0.7 102 (10%) 0.8 II 0.1 (1 203 (20%) 0.5 II 305 (30%) 0.6 0.3 II 0.6 406 (40%) 0.3 11 0.7 508 (50%) 0.3 _ II w u 0.7 610 (60%) 0.8 0.3 0 711 (70%) 0.8 0.3 0 813 (80%) 0 0.9 0 914 (90%) 1) 1016 (100%) 0.9 0.3 0 Table H.20. Results for Joint Space Experiment 2  ill  1111  llii  •  111 111  Dev iation of J oint [10 rad] 3  2 0 0 0 0 0 0 0 0 0 0  3 1 0 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0  4 0  0 0 I) 0 1 0 .1 0 0 0 0 0 0 0 0 1 0 1 (1  5 1 1 1 1  HI 111 8IH 1 ]  1  1 2 1 1 1 1 1 1 1 1  6 1 2 3 3 3 3 3 3 3 3  0 0 0 0 0 0 0 0 0 0  1)  u 0 0 0 0 0 (1  II  The quintic planner provides a faster motion at all demanded speeds, and equivalent accuracy at the end points. Use of the industrial planner results in an increase in motion time as the demanded speed is increased above 610 mm/s (60% MMAS). This occurs due to the distance limitation. The industrial planner algorithm computes the time necessary to ramp up to and then down from the demanded speed. If the distance covered during these two ramps is larger than the required distance, the acceleration and deceleration limits are reduced proportionally such that the required distance is met, rather than lowering the peak speed reached along the path, as is done with the quintic planner. Since a longer time is required to ramp up to and down from 100% of MMAS than 90% of MMAS, for example, and both cases are limited by the distance, the motion at 100% of MMAS will take longer.  Appendix H  144  H.3.3. Joint Space Experiment 3  Point  Joint 1 [rad]  1  5K  18  Joint 2 [rad]  Joint 3 [rad]  Joint 4 [rad]  K  3K  ~~9  4  0  Joint 5 [rad]  Joint 6 [rad]  5K  0  36  n 2K lK lK 7* ~~A 9~ ~9~ 36~ ~36 36 Table H.21. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 3 2  | Quintic Planner  Industrial Planner Demanded Speed Along Path [mm/s (% of M M A S ) ]  Motion Time [s]  102  (10%)  5.4  5.4  203  (20%)  2.7  305  (30%)  2.9 2.0  406  (40%)  1.6  508  (50%)  1.4  610  (60%)  711  (70%)  813  (80%)  1.3 1.2 1.1  1.9 1.5 1.2 1.1 1.0 0.9 0.9  Dev iation of J oint [10 rad] 3  1  1 0 0 0 0 0 0 0  2  0 l 0  111 111 Hi i 0  1.1 0 0 1.1 1016 (100%) 0.8 0 0 Table H.22. Results for Joint Space Experiment 3 914  •  (90%)  0 0 0 0 0 0 0 0 0  3  0 1 1 0 0 0 0 0 1 <)  4  0 0 0 0 0 0 0 0  0  0 0  0 0  0 0 0 0 0 0 0  5  0 0 1 1 1 1 1 1  0  ]  2 2  1  1 1 1 1 2 2 2  0 1 1 1 1 1 1 1 1 1  6 I  1 1 1 1 1 1 ]  1  0 0 1 1 1 1 1 1 1  1 1 1 1 1  1  1  ]  1  The quintic planner provides a faster motion at all demanded speeds, and equivalent accuracy at the end points.  145  Appendix H  H.3.4. Joint Space Experiment 4  Point  Joint 1 [rad]  1  11* 36 5*  2  Joint 2 [rad]  Joint 3 [rad]  Joint 4 [rad]  11* 36  10*  2*  9  ~9~  15* 36  37* 36  0  Joint 5 [rad]  Joint 6 [rad]  *  11* 36  ~"9  *  0  ~~9 18 Table H.23. Coordinates of Joint Space Straight Line Start and End Points for Joint Space Experiment 4  Quintic Planner  Industrial Planner Demanded Speed Along Path [mm/s (% of M M A S ) ]  Motion Time [s]  Dev iation of J oint [IO rad] 3  1  2  3  4  102 (10%)  6.0  5.9  1  0  0  0  0  0  0  203 (20%)  3.1  0 0  0 0  1  406 (40%)  1.6 1.3 1.2  711 (70%)  1.8 1.5 1.3 1.2  813 (80%)  1.2  1.0  914 (90%)  1.1  1016 (100%)  1.1  0.9 0.9  0 0 0 0 0 0 0  0 0 o  0  2.2  0 0  0  305 (30%)  3.0 2.1  508 (50%) 610 (60%)  1 1  ill 111 0  Table H.24. Results for Joint Space Experiment 4  0 0 0 0 0 I)  0 0 0 0 0 0  0  iii  Hi 0 0 0  0 0 0 0 0 0 II  ill 0 0 0  Hi 111 H {)  1 1 1 1 2 2 1  2  5  1 1 2  0 1 1 2 ; 1 2 2 2 2 2 2 2 2 2 1 1  111  6  0 1 2 2 2 2 2 2 2 2  1 1 1 1 1 1 1 1  1  0 1  -1-  1  >  1  The quintic planner provides a faster motion at all demanded speeds, and equivalent accuracy at the end points.  Appendix H  146  H.4. Multiple Way-Point Joint Space Motion  Point Joint 1 [rad] Joint 2 [rad] Joint 3 [rad]  *  1  l  j A H  2*  10*  0  17*  *  *  *  18  18  9  71  7*  17*  ~T  *  *  *  18  18  ~~9  %  *  3~  9  K  171  7 71  571  3  ~18~  10*  6 0  "Ti 2* 9~ 0  Quintic Planner  Industrial Planner Demanded Speed Along Path [mm/s (% of MMAS)]  Motion Time [s]  (10%) (20%) (30%) (40%) (50%) (60%) (70%)  21.32  21.21  10.79  in rr  7.31  7.18  813 (80%)  5.58 • 5.43 4.55 ! 4.40 3.89  3.72  3.47  3.28  3.21  2.98  0  ~T8  9 ~~3~ Table H.25. Coordinates of Way-Points for Multiple Way-Point Joint Space Path ~6  102 203 305 406 508 610 711  Joint 6 [rad]  0  ~6  2*  5  Joint 4 [rad] Joint 5 [rad]  3.02 2.80 914 (90%) 2.88 2.69 1016 (100%) Table H.26. Motion Time for the Multiple Way-Point Path at Varying Demanded Speeds  The quintic planner consistently provides a faster motion at a given demanded speed.  11* 18 0  

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                        
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            src="{[{embed.src}]}"
                            data-item="{[{embed.item}]}"
                            data-collection="{[{embed.collection}]}"
                            data-metadata="{[{embed.showMetadata}]}"
                            data-width="{[{embed.width}]}"
                            async >
                            </script>
                            </div>
                        
                    
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0090209/manifest

Comment

Related Items