Smooth Time Optimal Trajectory Planning for Industrial Manipulators by Daniela Constantinescu M.Sc, Transilvania University, Brasov, Romania, 1995 B.Sc, University of Brasov, Romania, 1986 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 standarrj THE UNIVERSITY OF BRITISH COLUMBIA September 1998 © Daniela Constantinescu, 1998 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at the University of 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 scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Department of HEe.HA.fS | 'cAL BMAJNBE iQ /'NCI, The University of British Columbia Vancouver, Canada Date QcTofoeft IS 1^3 & ————————j DE-6 (2/88) Abstract This work proposes and demonstrates a strategy for planning smooth path-constrained time-optimal trajectories for manipulators. Such trajectories are obtained by limiting the actuator jerks required by the planned motion. Existing planning strategies incorporate the smoothness requirement either as smoothness of the actuator torques or as smoothness of the joint trajectories. The smoothness requirement is desirable for reducing strain on robot actuators while still requiring low cycle times. In this work, the trajectory smoothness is defined in the phase plane and the planning observes the limits on the actuator jerks. The solution proposed for determining the optimal trajectories consists of approximating the time optimal control problem by a nonlinear parameter optimization problem which is solved using the flexible tolerance method. It is shown that the approximate solution converges to the time optimal motion when the actuator jerks become very high. A number of simulations are performed to demonstrate the proposed strategy. These simulations show that actuator jerk limits have a negative impact on robot motion time, but they do not give any indication about robot trajectory feasibility. This aspect is studied through further simulations and experiments on an industrial robot. The results of this work show that the tracking accuracy is directly related to the actuator jerk limits. Therefore, it is necessary to impose such limits when planning feasible optimal trajectories. Finally, the performance of the smooth time optimal motion is compared to the performance of both the non-jerk limited optimal trajectory, as well as a smooth quintic trajectory. For similar actuator jerks and controller effort, the smooth path-constrained time-optimal trajectory results in ii I l l a significantly shorter motion time with nearly the same tracking accuracy as a quintic polynomial. Based on the results in this work, actuator jerk limits are shown to provide an improved method of achieving a compromise between high tracking accuracy, smooth joint behaviour, and optimal motion time. Contents Abstract ii Table of Contents jy List of Tables vii List of Figures xiviji Glossary xi Acknowledgements xiv 1 Introduction 1 1.1 Background 1 1.2 Time-Optimal Motion 3 1.3 Industrial Motivation 5 1.4 Research Objectives , 6 1.5 Thesis Outline 6 2 Problem Formulation 8 2.1 Path-Constrained Time-Optimal Motion 8 2.1.1 Characteristics 8 2.1.2 Practical Implementation Issues 13 2.2 Research Addressing the Issues in PCTOM 13 2.3 Smooth Path-Constrained Time-Optimal Control 17 2.3.1 Problem Formulation 18 iv CONTENTS v 2.3.2 Path Constraints 21 2.3.3 Torque Limits 23 2.3.4 Jerk Limits 26 2.3.5 Admissible States 29 2.3.6 System Dynamics 31 2.4 Summary 33 3 Methodology 34 3.1 SPCTOM Trajectory Characteristics 34 3.2 Approximating Functions in Robotics 37 3.3 Proposed Method 40 3.3.1 Basis Functions 40 3.3.2 Optimization Variables 41 3.3.3 Numerical Solution 45 3.4 Summary 48 4 Simulations 49 4.1 SPCTOM Planning 50 4.1.1 Parabolic Path 50 4.1.2 Circular Path 54 4.2 Discussion 60 4.3 SPTOM Performance Investigation 61 4.4 Summary 70 5 Experiments 72 5.1 Experimental Setup 72 5.2 Experimental Results 73 5.3 Summary 76 6 Conclusions and Recommendations 78 6.1 Summary 78 6.2 Recommendations 79 CONTENTS vi Bibliography 81 Appendices 86 A The Flexible Tolerance Method 86 A . l Overview of the F T M 86 A.2 Overview of the F P M 88 A. 3 Implementation Details 89 B Robot Dynamic Models 95 B. l Elbow Manipulator Dynamic Model 96 B.2 SCORBOT ER VII Dynamic Model 97 List of Tables 2.1 Robot parameters 12 2.2 Actuator parameters 28 4.1 Robot parameters (repeated) 50 4.2 Actuator parameters (repeated) 50 4.3 Location of the PCTOM switching points in the s — s plane 51 4.4 Optimal motion times for the elbow manipulator along the parabolic path 52 4.5 Optimal motion times for the elbow manipulator along the circular path 56 4.6 Optimal motion times for the elbow manipulator along the circular path when ad-ditional knot points are inserted 58 4.7 P C T O M and SPCTOM motion times for the examples in Sections 4.1.1 and 4.1.2. . 60 4.8 The SCORBOT ER VII Denavit-Hartenberg parameters 61 4.9 SCORBOT ER VII estimated masses and inertias 62 4.10 Actuator torque and jerk bounds for the SCORBOT ER VII 62 4.11 PCTOM, SPCTOM, and quintic trajectory motion times for the SCORBOT ER VII along the straight line path 64 5.1 Experimental results for the PCTOM, SPCTOM, and quintic trajectories 75 vii List of Figures 2.1 The elbow manipulator 10 2.2 P C T O M plotted in the s - s phase plane. . 11 2.3 P C T O M trajectory in the s — s phase plane 11 2.4 Actuator torques for the PCTOM trajectory 12 2.5 Admissible region in the i 2 - s plane 24 2.6 Torque limits in the s — s — s space and the VLC 25 2.7 Admissible region in the s - 's plane 26 2.8 Control constraints in the s — s — s space 28 2.9 Different jerk limits in the s — s — s space 29 2.10 Admissible states in the s — s — s space 30 2.11 SMVLC for different actuator jerk limits 32 3.1 Influence of the end slopes and the height of the trajectory on the motion time. . . . 37 3.2 Sample trajectories in the s — s plane and their motion times 41 3.3 Switching points of the PCTOM trajectory 42 3.4 The optimization variables 43 3.5 Motion time and its level curves with respect to several parameters varied over their ranges 44 3.6 Plot of a phase plane trajectory resulting from a point in the inadmissible region of the parameter space 44 3.7 The parameter space in the Flexible Tolerance Method. 47 4.1 P C T O M for the elbow manipulator along the parabolic path 52 viii LIST OF FIGURES IX 4.2 SPCTOM for the elbow manipulator along the parabolic path - Example 1 (medium jerk limits) 53 4.3 SPCTOM for the elbow manipulator along the parabolic path - Example 2 (large jerk limits) 53 4.4 SPCTOM for the elbow manipulator along the parabolic path - Example 3 (low jerk limits) 53 4.5 Actuator jerks for the SPCTOM in Example 2 and the P C T O M for the elbow ma-nipulator along the parabolic path 54 4.6 P C T O M in the s — s phase plane for the elbow manipulator along the circular path. 56 4.7 P C T O M for the elbow manipulator along the circular path 56 4.8 SPCTOM for the elbow manipulator along the circular path - Example 1 (medium jerk limits) 57 4.9 SPCTOM for the elbow manipulator along the circular path - Example 2 (high jerk limits) 57 4.10 SPCTOM for the elbow manipulator along the circular path - Example 3 (low jerk limits) • 57 4.11 Peak actuator jerks for the SPCTOM and the PCTOM for the elbow manipulator along the circular path 58 4.12 SPCTOM for the elbow manipulator along the circular path (additional knot points inserted) - Example 1 (medium jerk limits) 59 4.13 SPCTOM for the elbow manipulator along the circular path (additional knot points inserted) - Example 2 (high jerk limits) 59 4.14 SPCTOM for the elbow manipulator along the circular path (additional knot points inserted) - Example 3 (low jerk limits) 59 4.15 P C T O M trajectory in the s - s phase plane for the SCORBOT ER VII along the linear path 63 4.16 P C T O M for the SCORBOT ER VII along the linear path 65 4.17 SPCTOM for the SCORBOT ER VII along the linear path (additional knot points inserted) - Example 1 (medium jerk limits) 65 4.18 SPCTOM for the SCORBOT ER VII along the linear path (additional knot points inserted) - Example 2 (high jerk limits) 65 LIST OF FIGURES x 4.19 SPCTOM for the SCORBOT ER VII along the linear path (additional knot points inserted) - Example 3 (low jerk limits) 66 4.20 Quintic trajectory for the SCORBOT ER VII along the linear path 66 4.21 Controller tracking performance for the PCTOM, quintic, and SPCTOM trajectories. 67 4.22 Desired and simulated torques for the PCTOM trajectory. 67 4.23 Desired and simulated torques for the SPCTOM trajectory - Example 1 (medium jerk limits) 68 4.24 Desired and simulated torques for the SPCTOM trajectory - Example 2 (high jerk limits) 68 4.25 Desired and simulated torques for the SPCTOM trajectory - Example 3 (low jerk limits) 69 4.26 Desired and simulated torques for the quintic trajectory 69 5.1 Experimental setup 73 5.2 Experimental results for the PCTOM trajectory implemented on the SCORBOT ER VII. 74 5.3 Experimental results for the SPCTOM trajectory (Example 1 - jerk limits of lOONm/sec) implemented on the SCORBOT ER VII 74 5.4 Experimental results for the SPCTOM trajectory (Example 2 - jerk limits of lOOONm/sec) implemented on the SCORBOT ER VII 74 5.5 Experimental results for the SPCTOM trajectory (Example 3 - jerk limits of lONm/sec) implemented on the SCORBOT ER VII 75 5.6 Experimental results for the quintic trajectory implemented on the SCORBOT ER VII 75 5.7 Experimental results for the SPCTOM trajectory (jerk limits of 20Nm/sec) imple-mented on the SCORBOT ER VII 77 5.8 Experimental results for the SPCTOM trajectory (jerk limits of 50Nm/sec) imple-mented on the SCORBOT ER VII 77 A . l The line searches during the replacement of the different types of infeasible points generated by the F P M 92 A.2 Flow diagram of the F T M for the SPCTOM problem - Part 1 93 A.3 Flow diagram of the F T M for the SPCTOM problem - Part 2 94 Glossary s The path parameter. s The first time derivative of the path parameter, the pseudo-velocity. s The second time derivative of the path parameter, the pseudo-acceleration. 's The third time derivative of the path parameter, the pseudo-jerk. q The column vector of joint variables. q The first time derivative of the joint positions, the vector of joint velocities, q The second time derivative of the joint positions, the vector of joint accelerations, q The third time derivative of the joint positions, the vector of joint jerks. M The inertia matrix of the manipulator. C The Christoffel's symbols (centripetal and Coriolis coefficients). G The vector of gravity terms. r The Cartesian coordinates of the end-effector. T The vector of actuator torques. Tmin The vector of minimum actuator torques. Tmax The vector of maximum actuator torques. T The first time derivative of the actuator torques, the vector of actuator jerks. Tmin The vector of minimum actuator jerks. Tmax The vector of maximum actuator jerks. A The coefficients of the pseudo-acceleration in the inequalities expressing the actuator torque constraints in the phase space. xi GLOSSARY xii B The coefficients of the squared pseudo-velocity in the inequalities expressing the actuator torque constraints in the phase space. C The free term in the inequalities expressing the actuator torque constraints in the phase space. a The coefficients of the pseudo-jerk in the inequalities expressing the actuator jerk constraints in the phase space, b The coefficients of the pseudo-velocity and pseudo-acceleration product in the inequalities expressing the actuator jerk constraints in the phase space, c The coefficients of the third power of the pseudo-velocity in the inequalities expressing the actuator jerk constraints in the phase space, d The coefficients of the pseudo-velocity in the inequalities expressing the actuator jerk constraints in the phase space. smax,T Maximum admissible pseudo-velocity when only the actuator torque constraints are considered. Smin,T Minimum admissible pseudo-acceleration when only the actuator torque constraints are considered. smax,T Maximum admissible pseudo-acceleration when only the actuator torque constraints are considered. Smax,J Maximum admissible pseudo-velocity when only the actuator jerk constraints are considered. Smin,j Minimum admissible pseudo-acceleration when only the actuator jerk constraints are considered. smax,J Maximum admissible pseudo-acceleration when only the actuator jerk constraints are considered. "s'min Minimum admissible pseudo-jerk when the actuator jerk constraints are considered. Smax Maximum admissible pseudo-jerk when the actuator jerk constraints are considered. GLOSSARY xiii / The optimization objective function (motion time) hi The optimization equality constraints. 9i The optimization inequality constraints. X A point in the parameter space. $ The flexible tolerance criterion. T The measure of the constraint violation. TOC Time Optimal Control. OCT Optimal Control Theory. PMP Pontryagin's Maximum Principle. T P B V P Two-Point Boundary Value Problem. dof Degrees of Freedom. C.O.M. Center of Mass. DH Denavit-Hartenberg. P C T O M Path-Constrained Time-Optimal Motion. V L C Velocity Limit Curve. SPCTOM Smooth Path-Constrained Time-Optimal Motion. JL-VLC Jerk-Limited Velocity Limit Curve. SMVLC Smooth Motion Velocity Limit Curve. F T M Flexible Tolerance Method. F P M Flexible Polyhedron Method. DFP Davidon-Fletcher-Powell. ORTS Open architecture Real-time Operating System. LQG Linear Quadratic Gaussian. PID Proportional Integral Derivative. RMS Root Mean Square. Acknowledgements My thanks go to: • My supervisor, Dr. Elizabeth Anne Croft, for her guidance, expertise and encouragement during the course of this work, and for never getting tired of proofreading this thesis. • Dr. Beno Benhabib of the University of Toronto, for his invaluable discussions regarding this work. • Kaan Erkorkmaz, for providing continuous support for the ORTS and always having an answer to my questions. • Gord Wright, for making my experimental setup a reality. • Al l my colleagues in the IAL and MAL labs at UBC, for their support and good humour. • My husband and my family, for everything. I also acknowledge the financial support of the Natural Sciences and Engineering Research Council of Canada and the Faculty of Graduate Studies at UBC. xiv Chapter 1 Introduction 1.1 Background The Robotic Industries Association defines a robot as "a reprogrammable multi-functional ma-nipulator designed to move materials, parts, tools, or specialized devices, through variable pro-grammed motions for the performance of a variety of tasks" [3]. Both mobile robots and fixed base manipulators conform to this definition. In this work, however, the focus is on fixed base manipulators commonly found in industry, and herein described as "industrial robots". By the above definition, a robot may be decomposed into two parts: (i) a mechanism (namely, joints and links) together with actuators, capable of performing an infinite range of motions; and, (ii) computer algorithms together with a control and a sensing system, capable of planning and controlling these motions. Since the issues involved in motion control are complex, they are typ-ically made more tractable by dividing them into motion planning problems and motion tracking problems. Motion planning may be done either on-line, while the robot is executing the task, or off-line, that is, before the execution starts. On-line planning is often more desirable, since it increases the robustness of the robot's behaviour in the face of uncertainties and modelling errors. However, off-line planning is used when the required planning algorithms are computationally intensive. Depending on the specification of the task, motion planning entails either planning a trajectory along a specified path, or, planning both a path and a trajectory along the path. A trajectory is 1 1.1 Background 2 a curve in state space that describes the system evolution in time. The trajectory incorporates the system dynamics. A path is a curve in Cartesian space (or in joint space) that describes the spatial evolution of the end-effector (or the joints). The path reflects the geometric constraints on the task, for example, the joint limits. Robot motions are specified in two different ways. When the end-effector is constrained to a predetermined path, the motion is called path-constrained. Applications of path-constrained manipulation include welding, gluing and spraying. Since the path is imposed by the task, only trajectory planning is addressed. When only the initial and final positions (and velocities) of the end-effector are imposed, the motion is called point-to-point. Pick and place operations are point-to-point manipulations. In this case both a path and a trajectory must be determined. Path planning is now subsumed by trajectory planning. However, this global problem is difficult due to the highly nonlinear and coupled dynamics of articulated robots. Most solutions are limited to two degree of freedom (dof) manipulators due to the exponential increase in complexity of the problem with increasing dof. Such solutions are generally considered only in off-line planning schemes when obstacles are not a concern. More often, however, path planning and trajectory planning are treated as independent problems and addressed separately. For example, this is the case when the trajectory is planned on-line or when the working environment is cluttered. Once the trajectory is determined by the motion planner, it is sent to the tracking controller. This low level controller ensures that the robot's position and velocity match the reference signals. While many different advanced controllers have been proposed, most often in industrial practice a simple PID controller is used. Although such a controller does not consider the nonlinear robot dynamics, at high control loop frequencies it can keep the manipulator reasonably close to a typical industrial trajectory. This approach usually requires the robot to operate well under its full dynamic capabilities. Thus, in the general context of motion planning, the present research is concerned with de-termining industrially feasible time-optimal trajectories for path-constrained tasks. These are tra-jectories which allow the robot to move in an optimal manner within the constraints of the robot actuators and controller capabilities. 1.2 Time-Optimal Motion 3 1.2 Time-Optimal Motion Since any manipulation can be performed in many ways, a desirable choice is to have the robot execute the motion in an optimal manner according to some relevant criterion. Optimality criteria for industrial manipulation tasks include: minimum energy consumption [50], [64], [65], [66]; minimum jerk [33], [34], [57]; and minimum effort [40]. However, the economic drive for maximum task productivity is so strong that much more research has been dedicated to time-optimal motions [5], [11], [14], [24], [26], [31], [38], [45], [54], [55], [56], [58], [59], [60]. Planning minimum time motions for industrial manipulators is a time optimal control (TOC) problem for a highly nonlinear and coupled multi-degree of freedom dynamic system. Thus, no explicit closed-form solution may be expected from Optimal Control Theory (OCT) based on the application of Pontryagin's Maximum Principle (PMP). For nonlinear systems, OCT only provides necessary conditions for optimality. Hence, only a set of candidate controls can be deduced using the general theory. These difficulties stimulated the research in time-optimal motion planning. Different algorithms have been developed for solving the problem and much research has been concerned with studying the structure of the solution; see, for example, [5], [11], [12], [23], [41], [60], [61]. This research has shown that the optimal controls are either of the bang-bang or of the bang-singular-bang type. The singular controls are not directly characterized by PMP. The first near-time optimal solution was proposed by Kahn and Roth [31] for point-to-point motions. They linearized the differential equations describing the dynamics of the robot, applied a transformation to decouple the linearized dynamics and then solved the TOC problem analytically. Hollerbach [29] showed that the coupling terms could not be neglected in the dynamics. That is, a manipulator is intrinsically nonlinear and coupled, and thus does not behave like a linear system slightly perturbed by nonlinearities. Since then, methods for obtaining the time-optimal motion along a specified path have been reported by Bobrow et al. [5], and Shin and McKay [55]. Both these methods maximize the velocity of the manipulator along the given path and give the result in terms of a switching curve in the phase plane. Using essentially the same algorithm, Pfeiffer and Johanni [45] and Slotine and Yang [59] improved the efficiency of determining the switching points. Shiller and Lu [54] extended 1.2 Time-Optimal Motion 4 the method to deal with singular controls. I For point-to-point manipulations, two approaches have been pursued. In the first approach, the TOC problem is solved globally, using either OCT or search techniques. In the former approach, PMP is applied to the Hamiltonian formulation of the robot dynamics [14], [13], [24], and the resulting two point boundary value problem (TPBVP) is solved by shooting methods. Alternatively, joint-space [47] or state-space [19], [26] search techniques are employed. In these methods, the space between the initial and final position (state) is tessellated and the linearized robot dynamics are used to compute the motion time between the tessellation points. The time-optimal trajectory is then found by a graph search. Regardless of the approach, the global TOC problem for point-to-point motions has been demonstrated only for simple two degrees of freedom manipulators in uncluttered environments. In the second approach, path planning and trajectory planning are treated separately. The path is restricted to a class of suitably chosen functions under the assumption that the optimal path is smooth. The methods devised for determining the optimal path include: a joint space search technique [46]; a parameter optimization technique [4]; the use of the dynamic properties of the manipulator [52]. The minimum time trajectory along the path is then determined using the algorithm in [5]. This disjunction of the global problem greatly simplifies its solution. This simplification has been exploited to plan motions for realistic manipulators, with more than two degrees of freedom [4]. It has also been used in on-line planning schemes [16]. In all the above works, manipulation tasks are planned in a time-optimal fashion. Hence, all of them require bang-bang or bang-singular-bang actuator torques. Such torques cannot be achieved by physical robots. Thus, a need arises for smoothing the controls before such minimum time strategies can be practically implemented. This need has been recognized for more than 15 years [38], [64], and it has been addressed both at the planning level and at the tracking level. At the tracking level, trajectory preshaping [51] and filtering of the control signals [37] have been used to smooth the optimal controls required by the planned minimum time trajectories. At the planning level, actuator voltages as controls [63] and cost functions including actuator effort [8], [37], [50], [66] have been used to generate trajectories which result in smooth optimal controls. This is done by taking into account a more complete model of the manipulator dynamics. The resulting trajectory is then feasible for implementation by existing (non-specialized) industrial robot controllers. 1.3 Industrial Motivation 5 In this work, smooth time optimal control is achieved through the second approach, namely, at the planning level. However, rather than require a more rigorous model of the robot (which is generally unavailable), constraints on the allowable actuator jerks are applied. Again, the objec-tive is to generate optimal trajectories that are feasible for implementation on existing industrial manipulators. 1.3 Industrial Motivation The research in time optimal robot control addresses the industry's demand for increased pro-ductivity. Whenever the process speed is limited by the robot itself, rather than by the task that it is performing, minimum time path tracking results in reduced cycle time. Shorter cycle times can increase productivity and, thus, decrease the cost per operation. To achieve the goal of time optimal control, both the trajectory planner and the tracking controller are designed to exploit the full capability of the robot. This ensures minimum execution time for the task. However, the algorithms, both at the planning and at the tracking level, use only a simplified model of the manipulator. Link flexibility, actuator dynamics and joint friction are usually ignored and the robot dynamic model is considered to be accurately known. These modeling assumptions are required by the complexity of the motion planning problems. However, such assumptions are problematic in the implementation of time optimal control to real systems. If implemented on a physical manipulator with a non-specialized controller, time optimal control will result in vibrations in the links and strain and wear on the actuators. Premature wear of the mechanical structure and frequent failures of the actuators can result [37]. Hence, in practical terms, the implementation of time optimal motion could decrease the return on capital investment in the robot. Furthermore, the productivity of the entire workcell may be reduced due to the down time for robot repairs. Such events defeat the purpose of time optimal control. Thus, from an industrial point of view, the need arises for a more realistic optimal control: a control that achieves a desirable balance between the minimum task completion time and the practical limitations of the robot system. 1.4 Research Objectives 6 1.4 Research Objectives The view adopted herein is that increased productivity, namely shorter time for performing a task, is important from an economic perspective. Nonetheless, the physical limitations of the robot, such as link and joint flexibility, must be acknowledged in an industrial implementation. Therefore, the general objective herein is to determine feasible optimal trajectories for path-constrained tasks. The benefit of such trajectories is that, while being time-optimal, they will also be suited for direct implementation on a commercial robot using non-specialized industrial controllers. In this context, the specific objectives are: (i) formulation of the TOC problem for path con-strained tasks such that it includes constraints through which different degrees of trajectory smooth-ness can be imposed; (ii) development of an algorithm for solving this problem; and, (iii) establishing that this algorithm results in improved robot performance, as measured by the tracking accuracy. 1.5 Thesis Outline This chapter introduced robotic motion planning and the time-optimal motion research. In summary, the practical limitations of minimum time motions led to the industrial motivation of the present work. The research objectives were presented and an outline of the thesis is given herein. Chapter 2 discusses the issues involved in the physical implementation of path-constrained time-optimal motions (PCTOM). Then, existing methods of addressing these issues are presented. Finally, a new formulation of the PCTOM planning problem is presented, in which smoothness requirements on the motion are incorporated as limits on the rate of variation of the actuator torques. In'Chapter 3, the methodology for the solution of the proposed formulation is described. As the TOC problem is transformed into an optimization problem, this transformation is justified and detailed. Simulations are performed using this optimization and the results are presented and discussed in Chapter 4. 1.5 Thesis Outline 7 An optimal trajectory planned using the algorithm developed in this work and a PCTOM tra-jectory planned according to the algorithm in [45] are implemented on the SCORBOT ER VII robot in the IAL laboratory. The experimental results and a comparison between robot performances along the two trajectories are given in Chapter 5. Finally, Chapter 6 presents the conclusions of this research together with the recommendations for future work. Chapter 2 Problem Formulation This chapter discusses time-optimal control of manipulators along specified paths and highlights the issues arising in the practical implementation of such control. Existing methods of addressing these issues, both at the trajectory planning and at the trajectory tracking level, are presented. A new formulation for time-optimal control is then introduced and its advantages summarized. 2.1 Path-Constrained Time-Optimal Motion Path-constrained time-optimal motions (PCTOM) are minimum time motions along a prespec-ified path. At the planning stage, a joint torques history is derived considering actuator torque limits, joint angle and joint velocity limits. The manipulator is modelled as a serial chain of rigid links, with each joint individually powered. The friction in the joints and the actuator dynamics are most often neglected. At the tracking level, a controller is designed that best can track the planned trajectory. Hence, the robot dynamic capabilities are used at the maximum in P C T O M . 2.1.1 Characteristics Path-constrained time-optimal trajectories have been found both by using dynamic program-ming [56], [58], or by identifying the switching points in the position-velocity phase plane [5], [45], [55]. The advantage of the dynamic programming approach is that the path need not be parame-8 2.1 Path-Constrained Time-Optimal Motion 9 terized. Also, performance indices other than minimum time may be easily incorporated. However, the phase plane approach is computationally very efficient and has been used in on-line planning schemes [16]. In the dynamic programming approach, the state space is discretized into a grid of points. At each point, the next allowable state-points on the grid are defined by imposing velocity and acceleration constraints [58], or velocity, acceleration and jerk constraints [56]. The cost of moving to each of the allowable state-points is the motion time. This cost is computed assuming that the acceleration is constant over one step. The dynamic programming algorithm is then applied to determine the minimum-time trajectory over all of the points. In the phase-plane approach, the path parameter and its first time derivative, the pseudo-velocity, are taken as the state. The second time derivative of the path parameter, the pseudo-acceleration, is the control. The nonlinear robot dynamics and actuator constraints are transformed into state-dependent constraints on the control. For each point along the path, the constraints also determine a maximum allowable end-effector pseudo-velocity such that the actuators can keep the end-effector on the path. This maximum pseudo-velocity as a function of the path parameter is called the velocity limit curve. The optimal trajectory is derived by selecting the admissible control that produces the largest pseudo-velocity at each point along the path, without violating the velocity limit curve. The solution is given in terms of a switching curve in the phase plane and an efficient method for identifying the switching points is provided in [45]. A method for dealing with the case of singular control is given in [54]. In all of the above cited works, one actuator torque is always saturated. Bobrow et al. [5] and Shin and McKay [55] have shown that the optimal control takes values only on the bounds of its admissible set. Bobrow et al. [5] have also shown that, as a consequence, at least one actuator torque is always saturated. Hence, the optimal policy requires that at least one actuator torque switches value instantaneously between saturation levels. An example is presented next to illustrate this bang-bang nature of the optimal actuator torques. A typical PCTOM trajectory of a 3-dof manipulator (Figure 2.1) and the velocity limit curve (VLC) are plotted in Figure 2.2 in the s-s phase plane, where s is the path parameter and s the 2.1 Path-Constrained Time-Optimal Motion 10 end-effector pseudo-velocity. The robot kinematic and dynamic parameters are taken from [45] Figure 2.1: The elbow manipulator. and are given in Table 2.1. In Table 2.1, C.O.M. is the center of mass of link i with respect to frame i and Ix, Iy, Iz are the central moments of inertia of link i with respect to axes parallel to the axes of frame i. The prescribed path, taken from the same source, is a parabola given in parametric form as: x(s) = 0.5 y(s) = 20 • s 3 - 30 • s2 + 10 cdots (2.1) z{s) = s-0.5 s = 0 . . .1. The algorithm for computing the trajectory is also from [45]. As shown in Figure 2.2, the PCTOM pseudo-velocity is continuous along the entire path and differentiable everywhere except at the five switching points. These are the points along the trajec-tory where the pseudo-acceleration, s, switches from maximum to minimum or vice-versa. Plotting 2.1 Path-Constrained Time-Optimal Motion 11 PCTOM VLC o o switching points 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 path parameter [m] Figure 2.2: PCTOM plotted in the s — s phase plane. k- chatter <- discontinuity discontinuity - * ; discontinuity -> -] •<- discontinuity 0.1 0.2 0.3 0.4 0.5 0.6 path parameter [m] 0.7 0.8 0.9 Figure 2.3: PCTOM trajectory in the s — s phase plane. 2.1 Path-Constrained Time-Optimal Motion 12 200 150 " i 1 1 r ACTUATOR 1 ACTUATOR 2 ACTUATOR 3 0.3 0.4 0.5 0.6 0.7 0.8 0.9 path parameter [m] Figure 2.4: Actuator torques for the PCTOM trajectory. Table 2.1: Robot parameters Link [m] C.O.M. [m] Mass [kg] Ix[kgm2] Iy[kgm2] Iz[kgm2] h = 0.00 lcl = 0.05 mi = 0.0 Ixi = o.oo lyl = 5.0 hi = o.o l2 = 0.75 lC2 = 0.20 TO2 = 6.6 1x2 = 0.10 Iy2 = 0.6 Iz2 = 0-6 h = 0.75 lc3 = 0-15 m 3 = 4.2 1x3 = 0.02 Iy3 - 0.2 hz - 0.3 the same PCTOM trajectory in the s-s phase plane, Figure 2.3, shows the discontinuities in the pseudo-acceleration at the switching points. The actuator torques necessary to generate this motion are plotted in Figure 2.4. These figures illustrate one of the problematic characteristics of the PCTOM trajectory: the pseudo-acceleration and the motor torques have discontinuities at the switching points and chatter occurs when the trajectory in the s-s phase plane touches the VLC. 2.2 Research Addressing the Issues in PCTOM 13 2.1.2 Practical Implementation Issues Time-optimal trajectories such as the one presented in the previous section represent a measure of the maximum robot tip velocity at which the actuators can still keep it along the path. However, as shown, they require instantaneous changes in joint accelerations and in joint actuator torques. Such discontinuities in actuator torques reflect the system model used in computing the PC-T O M trajectories. Namely, the robot links are considered to be completely rigid and the actuator dynamics are ignored. While these modelling assumptions are sufficient for low speed trajectories, they can be expected to have an impact on a minimum time motion. As a result of these assumptions, two negative effects arise, and each effect compounds the other. First, since the actuators cannot generate discontinuous torques due to their own internal dynamics, the joint output lags behind the reference signal. This decreases the tracking accuracy, and activates the tracking controller. Thus, after each switching point, the joint actuators exhibit chatter. Second, this chatter of the joint actuators induces vibrations in the robot links. These vibrations aggravate the wear on the robot and reduce the tracking performance. As a result, the tracking controller is activated more often and additional strain is imposed on the actuators. Vibrations are a fundamental limiting factor in any high speed manipulation. They result in robot wear and reduce the life span of the actuators. In an industrial application, where the robot must perform the same task hundreds of times per day, fast motions are desirable, but the structural wear and actuator lifetime are also important. The typical industrial solution is to detune the robot feedback controller by limiting its bandwidth so as not to excessively excite these vibrations. In path-following manipulations, however, the tracking performance may be adversely affected by limiting the frequency content of the control signal and other solutions may be required. Thus, all of these issues must be addressed in an industrial implementation of P C T O M trajectories. 2.2 Research Addressing the Issues in P C T O M The practicalities involved in path-constrained time-optimal control were recognized shortly after efficient algorithms for trajectory computation were developed and the bang-bang nature of the controls was proven. Methods have been devised for addressing these problems both at the 2.2 Research Addressing the Issues in PCTOM 14 tracking controller level and at the trajectory planning level. A fundamental problem associated with time-optimal control is to ensure a prescribed tracking accuracy. This is the objective of any path following manipulation task. The tracking performance is affected by unmodelled link flexibility and joint friction and disturbances occurring during the performance of the task. Nonetheless, algorithms have been proposed that consider only the effect of disturbances on the tracking performance. Since, in time-optimal control, one actuator is always saturated while the other actuators adjust their values to keep the end-effector on the path, there is no control input available to compensate for modelling errors and disturbances. Thus, performance deteriorates when unexpected control saturation occurs. Improved tracking accuracy may be guaranteed from the planning stage by keeping some torque in reserve [7], hence determining near-time optimal trajectories. Time-optimal trajectories with increased tracking performance may be obtained by accounting for the actuator dynamics. This approach is adopted in Tarkiainen and Shiller [63]. In their work, the actuator constraints are imposed as limits on the motor voltages. The system consisting of the robot and its actuators is a third order dynamic system with the pseudo-jerk, the third time derivative of the path parameter, as the control variable. The time-optimal policy is shown to be bang-bang or bang-singular-bang in the pseudo-jerk and an algorithm is given to determine the optimal trajectory. The algorithm ignores the case of singular controls, since these can be avoided by appropriately choosing the path [54] or by convexifying the set of admissible controls [49]. The optimal torques are continuous in this formulation and the trajectory is smooth. However, the method does not allow the path planner to directly impose a desired degree of trajectory smoothness. The concern for improving the tracking performance exists not only at the planning, but also at the tracking level. Solutions proposed at the controller level include: (i) a secondary controller for on-line trajectory time scaling [17]; (ii) an LQG controller for adjusting the switching times on-line based on the measured performance of the system [62]; (iii) a closed-loop trajectory generator that is robust to experimentally identified levels of disturbance [32]; (iv) a trajectory preshaping scheme that compensates for actuator dynamics [51]. Although these methods increase the tracking accuracy, they do not directly address the problem of discontinuous reference torques or completely compensate for the resulting transient system behaviour. In [51], however, the overshoot of the nominal torques and the control chatter after each bang are acknowledged. It is pointed out that 2.2 Research Addressing the Issues in PCTOM 15 the overshoot is quite significant and requires that conservative bounds on the admissible torques be used during the planning stage. While good tracking accuracy is highly desirable for path-constrained motions, robot wear and the life span of the actuators are important considerations in any industrial application of time-optimal control. Unfortunately, it is much more difficult to measure the cumulative effect that a specific algorithm may have on vibrations or wear. Acknowledgements of this issue exist in the literature [8], [33], [34], [38], [45], [50], [56], and it is recognized that, in practice, some degree of smoothness is necessary. However, in this review, only one method was identified for generating and evaluating a smooth trajectory which explicitly considers vibrations [66]. This method is based on the iterative adjusting of the weights in the cost function till a smooth input is obtained that results in reduced vibrations in the joints and no oscillations at the end-point of the trajectory. At the planning level, the smoothness requirement may be considered with respect to the trajectory smoothness or the smoothness of the actuator torques. Since the actuator torques are computed as continuous functions of the trajectory parameters, a smooth trajectory translates into smooth control signals. In Lin et al. [38], smooth trajectories are planned by choosing cubic polynomials to approximate the joint paths. This results in trajectories with continuous joint velocities and accelerations. The motion time is then minimized subject to the physical constraints on joint velocities, accelerations, and jerks. The limits used are conservative, because they are constant over the entire robot work space. Hence, they are determined for the worst case, i.e. the arm fully extended (maximum inertia). The same joint path parameterization is used in Cao et al. [8], together with a quadratic cost function involving time and joint accelerations. The limits on the joint variables are imposed after the optimization is solved, through time scaling. The resulting motion has optimal joint velocity profiles, in the sense that the variance of the joint accelerations is minimum in the natural motion, i.e. with zero initial and final states. Then, for the same limits on the motion acceleration, the motion vibration is minimum. As a result, the strain energy and the mechanical wear are approximately minimal. The actuator torques may also be smoothed directly by adding an "energy" term to the objective 2.2 Research Addressing the Issues in PCTOM 16 function [45], [50], [56], [64], [66]. This term convexities the cost and the Hamiltonian with respect to the control and creates a minimum that is not necessarily on the control boundaries. In [56] constraints on the joint jerks are imposed explicitly, while the algorithms in [50] and [66] allow weighting of the energy term. By modifying this weight, Shiller [50] achieved a desired compromise between minimum time and increased accuracy, while Wappenhans et at. [66] obtained motions with reduced low-frequency vibrations and no residual oscillations. At the tracking and control level, Newman [43] developed a feedback control scheme as a combination of bang-bang control with sliding-mode control. This feedback law drives the system to imitate the desirable and feasible set of dynamics, which are specified as a state constraint corresponding to the motion along the switching curve of the time-optimal control. The concern with unmodelled dynamics is explicit. An analysis in terms of a design parameter of the controller is performed. This analysis shows how the parameter should be chosen such that high-frequency dynamics of the system does not destabilize the controller. Thus, the proposed controller is robust to parametric uncertainty and unmodelled flexibility of the robot structure. L i et al. [37] explicitly acknowledged vibrations as the limiting factor in time-optimal control. Ways of compromising between minimum motion time, and robot wear and actuator lifetime for point-to-point motions were considered in their work. Both vibrations and wear were addressed in two ways: (i) by planning time-optimal motions together with different torque parameteriza-tions; (ii) by using cost functionals involving terms with various exponential powers of the actuator torques with piecewise linear, continuous controls. By penalizing the terms containing powers of the actuator torques, varying degrees of smoothness of the controls were achieved. Two strategies were recommended for time-optimal control. One involved prefiltering the time-optimal torques and then matching the bandwidth of the low-pass filter to the bandwidth of the feedback controller. The other utilized the derivative of the torque as the control. However, the lowest vibration levels corresponded to a quadratic cost functional involving the second derivative of the torque. In summary, actuator chatter, large overshoot of the nominal torques, decreased tracking ac-curacy, vibrations, and robot wear arise in the direct implementation of P C T O M trajectories due to the bang-bang nature of the actuator torques required by such trajectories. These difficulties have been addressed by a large body of research, and methods have been devised for overcoming these problems, both at the trajectory planning level and at the tracking controller level. At the 2.3 Smooth Path-Constrained Time-Optimal Control 17 controller level, solutions include: sophisticated controllers, more accurate models of the robot-actuators system, filtering of the reference torques, and parameterizations of the control signals. However, such approaches do not directly address the cause or completely solve the effects of these difficulties. At the planning level, proposed solutions include smooth joint path parameterizations, objective functions incorporating an energy term, the use of actuator voltages as the controls, and the restriction of controls to some suitably chosen set of functions, i.e., piecewise linear and continuous or piecewise quadratic and continuous controls. In the following section, a new formulation is proposed for the planning of time-optimal trajecto-ries for path-constrained tasks. This formulation avoids the disadvantages of P C T O M trajectories by choosing the derivatives of the actuator torques as the controls. 2.3 Smooth Path-Constrained Time-Optimal Control The view adopted in this work and supported by the reviewed research is that physical manip-ulators cannot exhibit discontinuous actuator torques. When trying to achieve such a behaviour, robot links can be subjected to severe vibrations that may lead to increased robot wear and failure of the actuator themselves. Herein it is considered that it is important to address these problems from the trajectory planning level and not leave them entirely to the tracking controller. The reasons for advocating these measures at the planning level are twofold. First, eliminating the torque discontinuities from the planning stage will reduce the controller effort and thus better tracking performance and a longer life span of the actuators are expected. Also, more room is left for controller robustness in face of model uncertainties. This is important, especially considering that an accurate dynamic model of the robot is difficult to obtain, and all trajectory planning schemes rely on such a model. Second, since the planned trajectories will be free from the problems associated with P C T O M trajectories, it will be possible to implement them on industrial manipulators using their own "general purpose" controllers. Using this approach, minimum time remains the criterion for planning the trajectory. However, the physical limitations of the robot are considered by requiring that the optimal trajectory be smooth. In this work, trajectory smoothness is imposed through limits on the rate of variation 2.3 Smooth Path-Constrained Time-Optimal Control 18 of the actuator torques, hereafter termed actuator jerks. Thus, different degrees of smoothness of a trajectory may be achieved by adjusting the limits on the actuator jerks. The time-optimal trajectory resulting when both actuator jerk and actuator torque limits are imposed will be called the smooth, path-constrained, time-optimal motion (SPCTOM). 2.3.1 Problem Formulation The SPCTOM planning problem is formulated hereafter for the case of a manipulation task starting and ending at rest. Thus, the problem that is addressed in this work is: min J Ten Jo = [ ' left, (2.2) Josubject to the standard model of the manipulator dynamics: , M(q).q + qT-C(q)-q+G(q) = T, (2.3) the boundary conditions: q(0) = q 0 ; q(</) = Of i q(0) = q(*/) = 0 ; q(0) = q(i/) = 0, (2.4) the path constraints: r = r(a), (2.5) the actuator torque limits: Tmin ^ T < T m a x (2-6) and the actuator jerk limits: Tmm — ^ — ^max (2-7) where T m j n , Tmax, Tmin, and Tmax are the actuator torque and jerk limits, respectively. Further-2.3 Smooth Path-Constrained Time-Optimal Control 19 more, q G. R n is the vector of joint positions, T G R" is the vector of actuator torques, T G R n is the vector of actuator jerks, M(q) G R n x n is the inertia matrix of the manipulator, C(q) G R n x n x n is a third order tensor representing the coefficients of the centrifugal and Coriolis forces, G(q) G R n is the vector of gravity terms, r G R 3 is a C 1 continuous curve parametrized by s, which may be, for example, the arc length, and n is the number of dof of the manipulator. For simplicity, viscous and static friction terms have been neglected in the dynamics. In the above formulation, the actuator jerks represent the bounded controls. However, these controls would later have to be integrated, in order to derive the actual system inputs in terms of desired actuator torques. Since the Lagrangian form of the robot dynamics incorporates only the actuator torques, the third order dynamics is derived. Differentiation of Equation (2.3) with respect to time results in: M(q). q>M(q).q + qT.C(q)-q + qT-C(q)-q + qT-C(q)-q+G(q)=T. (2.8) In terms of optimal control theory, the third order dynamics of the manipulator, Equation (2.8), are the dynamics of the system, with T representing the n-dimensional bounded controls. In the usual fashion, Equation (2.8) can be written in the form of the system state equation: x = F(x, u) = F(x) + K(x)u, (2.9) where the state of the system is: x — (q q q hx3n5 (2.10) while: P(x) = q q ^ - M - 1 (q)(M(q)q + qTC(q)q + qTC(q)q + qTC(q)q + G(q)) 3nxl (2.11) 2.3 Smooth Path-Constrained Time-Optimal Control 20 K(x) ( 0 ^ V M_1(q) L (2.12) and: u = T nxl- (2.13) Thus, the optimal control problem is reformulated as follows. Find the control T: T : [t0,t/] — • O C R ' 1 , (2.14) where CI is the set of admissible controls, that drives the system (2.9) in minimum time from an initial state: QO Olxn 0i> lx3n to a final given state: T QfT 0 i x n 0i> lx3n subject to the limits on the states: (2.15) (2.16) Trmn < M(q) ' Q + Q T • C(q) • q + G(q) < Tmax, (2.17) and the limits on the controls: Tmin ^ T < T^max- (2.18) Formulation of the problem solution using Pontryagin's Maximum Principle results in a T P B V P in a 3n-dimensional state space. 2.3 Smooth Path-Constrained Time-Optimal Control 21 2.3.2 Path Constraints The dynamic system described by Equation (2.8) has 3n degrees of freedom. However, the path constraints (2.5) parameterize the tip position by a single parameter s and reduce the order of the system to 3. By expressing the joint positions, velocities, accelerations, and jerks as functions of the path parameter s, the actuator torque limits and jerk limits are transformed into limits on s, the pseudo-acceleration, and's, the pseudo-jerk, respectively. The joint values corresponding to a given end-effector position and orientation, q = q(r) can be determined for each value of the path parameter s by solving the manipulator inverse kinematic equations and then substituting the path constraints (2.5): q = q(r(a)) (2.19) The dependence of the joint velocities q on the path parameter and its first derivative, the pseudo-velocity, is determined using the Jacobian matrix of the forward kinematic map: f = J(q)-q (2.20) If the manipulator is not at a singular position, the Jacobian is invertible and: q = J - i r = J " 1 ^ = J " 1 • r's = q's, (2.21) dsdt v 1 where: q' = J " 1 • r', (2.22) and ' denotes the derivative with respect to the path parameter. From Equation (2.21), the joint accelerations and jerks are obtained by successive differentia-2.3 Smooth Path-Constrained Time-Optimal Control 22 tions with respect to time: q = q's (2.23) - q = q"s2 + q's (2.24) q = q" ' i 3 + 3-q"-ss + q''5, (2.25) where q', q", and q"' can be derived using the inverse of Equation (2.22): r' = J q' (2.26) =^ r" = ^ . q ' + J . q » (2.27) ds Now, from Equation (2.27): q ' ' = J - i . ( r " - g . q ' ) , (2.29) and from Equation (2.28): q-» = J - 1 . ( r » _ ^ . q . _ 2 . = , . q » ) . (2.30) Substituting Equations (2.23), (2.24) and (2.25) in (2.8) yields the jerk equation: M • (q'" • s 3 + 3 • q" • s • s + q' • s ) + ^ • s • (q" • i 2 + q' • s) + as + (q" • s2 + q' • s)T • C • q' • s + qT • s • —-,•s • q' • s + ds + q ' T - ^ C - ( q " T - i 2 + q'-3) + ^ - i = T- (2-31) as The actuator jerk bounds become: Tmm < a(s) • a + b(a) • s • S + c(s) • s 3 + d(s) • s < Tmax, (2.32) 2.3 Smooth Path-Constrained Time-Optimal Control 23 where, a n x i ( s ) = M q ' , (2.33) b n x i (s) = 3-M-q" + ^ - q ' + 2-q' T -C-q', (2.34) as cnxl(s) = M • q'" + ^ • q" + q"r • C • q' + q'T • ^ • q' + q'T • C • q", (2.35) as as dG . , d„xi(s) = -7--s. 2.36 as The matrices ^ and ^ and the third order tensor ^ are robot dependent. Substituting Equations (2.23) and (2.24) into (2.3) yields the torque equation: M • q' • s + M • q" • s 2 + q'T • s • C • q' • s + G = T (2.37) and the actuator torque bounds become: Train < A{s) • s + B{s) • s2 + C{s) < Tmax (2.38) where: Anxl(s) = M q ' , (2.39) Bnxl(s) = M - ^ ' + q^-C-q', (2.40) Cnxl(s) = G. (2.41) 2.3.3 Torque Limits The actuator torque bounds (2.38) represent a set of 2n inequalities of the form: Ti^rnin ^ A{ • S + B{ • S + Cj < Ti^raaxi i = 1, • • • ,Tl , (2.42) where Ai, Bi, Ci, Titmin, and T^max are the elements of the vectors A(s), B(s), C(s), T m j n , and Tmax, respectively. For some value of the path parameter s, the feasible region of s and s, satisfying (2.42) and s > 0, forms a polygon in the s2 — s plane. Such a polygon is shown schematically in 2.3 Smooth Path-Constrained Time-Optimal Control 24 Figure 2.5 for a 3-dof manipulator. S max.T s 2 [m2/s2] Figure 2.5: Admissible region in the s 2 — s plane. As discussed in [54], for each value of the path parameter s, the bounds on the pseudo-velocity s and pseudo-acceleration s due to the torque constraints (2.38) are: 8 < Smax,r(s) (2.43) Smin,T{s,s) < S < S m a X t T ( s , s ) , (2.44) where: 8min,T(s, s) = max {t ( Ti — Ci — Bis , i = 1,... ,n (2.45) smax,T(s,s) = min {max d - B;S< , i = 1,... ,n (2.46) and %oi,r(«) = min <^ max i,j [Ti,Tj Aj(Tj - Q) - Aj(Tj - Cj) AjBi — A{Bj J j , i,j = 1,... ,n , (2.47) 2.3 Smooth Path-Constrained Time-Optimal Control 25 when Ai ^ 0 and AjBi — AiBj ^ 0. The subscript T is used to discriminate the pseudo-velocity and pseudo-acceleration bounds due to the torque constraints (2.38) from those due to the jerk constraints (2.32), which will be denoted with the subscript J. If Ai = 0 for some i , then this actuator does not bound the pseudo-acceleration at this point along the path. The limits are determined by the other n — 1 actuators. The case that A{ = 0 for all i is unlikely, since M is generally positive definite, except for some few particular cases [1], and q' is a vector tangent to the path. If AjBi — AiBj = 0, then the ith and jth actuators do not contribute to the velocity limit, or the maximum velocity is infinite. In the s — s — s phase space, the pseudo-acceleration bounds (2.44) represent a lower and an upper surface which meet and vanish along the VLC on the s — s plane. In Figure 2.6 , these surfaces have been plotted for the actuator torque limits given in Table 2.2. A l l the states inside these surfaces are states that obey the actuator torque constraints (2.17). 40 -s Figure 2.6: Torque limits in the s — s — s space and the V L C (dark line). The V L C represents an upper bound for any feasible trajectory in the s — s plane. Unless otherwise specified, all the figures in this and the following sections refer to the elbow manipulator with the dynamic parameters given in Table 2.1 moving along the parabolic path specified by Equations (2.1). 2.3 Smooth Path-Constrained Time-Optimal Control 26 2.3.4 Jerk Limits A similar approach can be used to determine the pseudo-velocity, pseudo-acceleration and pseudo-jerk bounds due to the actuator jerk limits. Thus, the actuator jerk bounds (2.32) rep-resent a set of 2n inequalities of the form: Titmin < CliS + b*S + C* < Tj.maz, « = 1, • • • , ™ (2-48) where a;, 6j, a, du Titmin, and fi>max are the elements of the vectors a, b, c, d, Tmin, and Tmax, respectively, and b* = biS and c* = C j S 3 + d,s. For a set of values of the path parameter s and the pseudo-velocity s, the feasible region of s and 's satisfying (2.48), forms a polygon in the s — 's plane. Such a polygon is shown schematically in Figure 2.7 for a 3-dof manipulator. As shown in Figure 2.7, for each set of values for s and i , the bounds on the pseudo-acceleration s and the pseudo-jerk's due to the jerk constraints (2.32) are: {s,s) <s<smax,j{s,s) (2.49) (2.50) 2.3 Smooth Path-Constrained Time-Optimal Control 27 where: s 'min (s, s, s) = max < min — ^ — > , i = l,...,n , (2-51) ( ( Ti — c* — b*s \ 1 max — - %— ]} , i = l,... ,n , (2.52) and •W I , J ( S , S ) = max 4 min J— >, z,j = l , . . . , n , (2.53) Smax,j(s,s) = mm < max — > , t,j = l,...,n , (2.54 l'i [Tift \ a A -aibj assuming that Gtj ^ 0 and — aj6* ^ 0. Again, the subscript J indicates that the s and 's bounds are due to the jerk constraints (2.32). If Oj = 0 for some i, then this actuator does not bound the pseudo-jerk at this point along the path. The limits are determined by the other n — 1 actuators. The case that Oj = 0 for all i is unlikely for the same reasons as in the case of the torque constraints, since a = A. If o-jb* — = 0, then the ith and jth actuators do not contribute to the pseudo-acceleration limits, or any pseudo-acceleration is admissible. This is the case with any manipulator starting from rest, for example. In the s — s — s space, the pseudo-acceleration constraints (2.49) represent two limiting surfaces which meet and vanish along a curve. The projection of this curve on the s — s plane is called the jerk limited velocity limit curve (JL-VLC). The limiting surfaces are plotted in Figure 2.8 for the first column of actuator jerk limits given in Table 2.2. The JL-VLC expresses the bound on the pseudo-velocity due to the jerk constraints (2.32): s < smax,j(s). (2.55) It represents an upper bound on any feasible trajectory in the s — s plane. For each value of the path parameter, this maximum allowable pseudo-velocity due to the jerk constraints is defined as 2.3 Smooth Path-Constrained Time-Optimal Control 28 Table 2.2: Actuator parameters Torque limits T[Nm] Medium jerk limits Example 1 fexl[Nm/sec] High jerk limits Example 2 fex2[Nm/sec} Low jerk limits Example 3 fex3[Nm/sec] Tx = 140 T2 = 140 T3 = 50 Tn = 500 T 2 i = 500 Tsi = 100 T12 = 5000 T 2 2 = 20 00 T 3 2 = 1000 Ti3 = 140 T 2 3 = 140 T 3 3 = 50 the s value at which the feasible region in the s — 's plane reduces to a point: Smin,j{si Smax,j) = Srnax,j(si Smax,j)- (2.56) This maximum can be computed by a numerical search which determines the value of s that satisfies Equation (2.56) for a given value of s. 2.3 Smooth Path-Constrained Time-Optimal Control 29 2.3.5 Admissible States In the formulation of the SPCTOM problem proposed herein, the actuator jerk limits are imposed as a means for adjusting the smoothness of the trajectory. Hence, they are independent of the actuator torque limits. This independence is shown in Figure 2.9, where the limiting surfaces due to the jerk bounds are plotted for the three example actuator jerk limits in Table 2.2. In Figure 2.10, the torque and the jerk constraints are plotted in the phase space for all three examples. (a) Example 1 (medium jerk limits) (b) Example 2 (high jerk limits) (c) Example 3 (low jerk limits) Figure 2.9: Different jerk limits in the s — s — s space. 2.3 Smooth Path-Constrained Time-Optimal Control 30 (a) Example 1 (medium jerk limits): meshed sur- (b) Example 2 (high jerk limits): meshed surfaces -faces - jerk limits ; shaded surfaces - torque limits. jerk limits ; shaded surfaces - torque limits. (c) Example 3 (low jerk limits): meshed surfaces -jerk limits ; shaded surfaces - torque limits. Figure 2.10: Admissible states in the s — s — s space. 2.3 Smooth Path-Constrained Time-Optimal Control 31 The independence of the actuator torque and jerk limits is reflected in a new constraint on the pseudo-velocity: s < min {smaXtT(s), smax,j{s)} , (2.57) and a new constraint on the pseudo-acceleration: max{s ' m i „ ) T (s) ,s m i n ! 7 (s)} < s < min{s'm a X )r(s),sm a X : J(s)}. (2.58) Equation 2.57 defines a global velocity limit curve, called the smooth motion velocity limit curve (SMVLC). In the s — s plane, the SMVLC is an upper bound on any feasible trajectory. Herein, the SMVLC is computed by a numerical search through bisection. The SMVLC corresponding to the three examples in Table 2.2 are plotted in Figure 2.11. Figure 2.11 demonstrates that the SMVLC is determined by a combination of both actuator torque and jerk limits. Depending on the restrictions of the jerk limits, they can determine the SMVLC almost entirely, as shown in the third example, or they can have little influence on it, as shown in the second example. 2.3.6 System Dynamics For the reduced system of order 3 where the path constraints (2.5) are incorporated into the dynamics (2.8), the states of the system are x = (s s s)T, while the pseudo-jerk 's is the scalar control u. The smooth time-optimal path-constrained trajectory planning problem can be formulated now as follows. Determine a scalar control: u : [t0,*/] —> UC R, (2.59) where U is the set of admissible controls, such that: rtf m i n J = / ldt, (2.60) 2.3 Smooth Path-Constrained Time-Optimal Control 32 Q 2 I I I ! I I I I I I ' 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 path parameter [m] Figure 2.11: SMVLC for different actuator jerk limits. subject to the system dynamics: the boundary conditions: 0 1 0 0 X = 0 0 1 •x + 0 0 0 0 1 (2.61) the state inequality constraints: xrj = (so so so) X / = (Sf Sf Sff , (2.62) s < mm{smax,T(s),smax,j(s)}, (2.63) 2.4 Summary 33 raax{smintT{s, s), sminrj(s, s)} <s< mm{'smax^(s,s),smaXyj(s,s)} (2.64) and the state-dependent control inequality constraints: min \ $ j 8, S ) < 's < '• (2.65) In this final formulation, the system dynamics is described by a system of three first order differential equations with constant coefficients. Thus, the complexity of the TOC problem is shifted from the system dynamics to the nonlinear state-dependent state and control constraints. 2.4 Summary P C T O M trajectories are not suited for direct implementation in an industrial application be-cause they require discontinuous actuator torques. A physical robot has unmodelled link flexibility and actuator dynamics. In this situation, bang-bang joint torques induce arm vibrations. These characteristics decrease the tracking performance in path following applications resulting in strain on the actuators and the mechanical structure. Wear becomes a concern. Recent research has addressed the need for smooth actuator torques both at the trajectory planning and at the trajectory tracking level. However, none of the reviewed works proposes a method for smoothing the torques individually, based on the dynamic characteristics of the robot and the path. Herein, the path-constrained time-optimal trajectory planning problem is reformulated such that the actuator jerks are the controls and limits on both joint torques and joint jerks are consid-ered. The actuator jerk limits may be chosen such that different degrees of smoothness are imposed on the actuator torques. The independence of the torque and jerk limits is reflected in the SMVLC, which is determined by both limits. The SMVLC is computed herein using a bisection search. Chapter 3 Methodology In this chapter, a methodology is proposed for solving the problem formulated in Chapter 2. Namely, determine the trajectory that drives the manipulator along a prescribed path from a given initial state to a final desired state in minimum time, subject to limits on the actuator torques and their rates of variation, the actuator jerks. This is the SPCTOM trajectory planning problem. Herein, this TOC problem is transformed into a nonlinear parameter optimization problem. Previous work on solving such optimal control problems through nonlinear programming techniques is reviewed and the flexible tolerance method (FTM) [27] is identified as a suitable candidate method for solving the SPCTOM problem. The formulation of the nonlinear optimization is presented and an overview of the F T M is given. 3.1 S P C T O M Trajectory Characteristics The SPCTOM problem as defined in Section 2.3.6 is a TOC problem for a first order lin-ear system with constant coefficients, nonlinear state-dependent state and control constraints and preimposed initial and final states. Such TOC problems have been solved either by applying the PMP to derive the necessary conditions for optimality and then using multiple shooting methods to solve the resulting two point boundary value problem (TPBVP) [6] or by a search for the switching points, using either dynamic programming [56] or specific algorithms [5], [45], [54], [59]. Two difficulties arise in the application of these approaches in the present case. First, the 34 3.1 SPCTOM Trajectory Characteristics 35 complexity of the dynamic programming algorithms grows exponentially with the phase space di-mension, rendering the method infeasible for more than two dimensions. As defined, the SPCTOM problem has a three dimensional phase space. Second, the other two approaches (based on PMP and the search for the switching points) depend on the bang-bang or bang-singular-bang structure of the optimal controls. This structure has been proven using results from OCT regarding systems with state dependent control constraints [35]. However, no results have been proven using OCT concerning the necessary optimality conditions for systems with state and control constraints which are independently active. Thus, for the SPCTOM problem, it is not guaranteed that the optimal controls take values only on the boundary of the admissible set. Hereafter, the SPCTOM trajectory planning problem will be analyzed and solved in the s — s plane. By doing so, another advantage is obtained besides surmounting the difficulties presented above. Namely, in the s — s plane both end points are fixed, while in the time domain the final point is free. Thus, the TOC problem lends itself to a nonlinear parameter optimization problem in this phase plane. To state the optimization problem, two results will be needed : (i) first, that any feasible trajectory, i.e., a trajectory complying with the constraints (2.63)-(2.65), is a smooth curve in the s — s plane, i.e., it has a continuous first derivative; and, (ii) second, that in the s — s phase plane, both the trajectory slopes at the path end-points and the height of the trajectory, i.e., the pseudo-velocity along the path, affect the motion time. These two results are established below. The smoothness of a feasible trajectory can be proved by considering the control constraints (2.65). These inequalities impose that, at any point along the trajectory, with the exception of the first and last point (when the manipulator is starting from or stopping at rest), the pseudo-jerk 's be limited. Then, the pseudo-acceleration s is continuous along the entire trajectory and so is the pseudo-velocity s. This result can be established considering a feasible trajectory in the s — s plane. Such a trajectory is a curve s(s). Therefore, its first derivative is 4f. Considering that: (3.1) 3.1 SPCTOM Trajectory Characteristics 36 then: ds ds s s (3.2) As both s and s are continuous functions along the entire trajectory, it follows that is continuous along the entire trajectory. The end points present no difficulty, since at these points both the pseudo-velocity and the pseudo-acceleration are zero. Thus, the values of ^ at these points can be obtained as: The second result needed for solving the SPCTOM problem is that the optimal trajectory motion time is determined both by its end slopes and its height in the phase plane. The influence of each of these two elements is illustrated in Figure 3.1(a) and 3.1(b). In Figure 3.1(a), two trajectories are shown that differ from each other over some path parameter interval due to the different initial slopes in the phase plane. Considering the identity: one can see that trajectory (i) results in a shorter motion time that trajectory (ii). Similarly, in Figure 3.1(b), the two trajectories have the same slopes at the end points, but (iii) is higher than (iv) over the path parameter interval, hence it has a shorter motion time. The combined effect of these two factors determines the motion time. In order to determine the optimal smooth curve in the s — s phase plane that does not vio-late actuator torque and jerk limits, two issues have to be addressed. First, the trajectory has to be approximated in some manner, such that it can be represented by a finite number of parame-ters. Second, an optimization technique must be selected to determine the optimal values of these parameters. (3.3) and (3.4) (3.5) 3.2 Approximating Functions in Robotics 37 1.4, , , , , , , , , , 1 i.4. path parameter [m] path parameter [m] (a) Trajectories with different initial slopes. (b) Trajectories with the same init ial slopes Figure 3.1: Influence of the end slopes and the height of the trajectory on the motion time. Herein, the trajectory is approximated by cubic polynomials. By this choice, the smoothness requirement is satisfied. Furthermore, there is a great deal of material available on using such polynomials for function approximation. 3.2 A p p r o x i m a t i n g F u n c t i o n s i n R o b o t i c s In optimal motion planning robotics research, approximating functions have been used for planning paths [4], [9], [10], [16], [20], [25], [30], [39], [40], [46], [53] and trajectories [28], [38], [48]. Using such functions, infinite dimensional optimization problems have been converted to finite dimensional optimizations. Theoretically, an infinite number of such approximating functions should be used. However, for practical purposes, approximations with a finite number of functions may still produce paths or trajectories sufficiently close to the true optimal ones. However, such an approach leads to further problems, namely : (i) the choice of the basis functions used in the approximation; (ii) treatment of the equality and inequality constraints; (iii) the choice of the optimization algorithm. These problems are usually interlinked and as such the choice of one affects the choice of the others. Generally speaking, however, cubic splines have been the basis functions of choice for path approximation. They are selected for their properties of 3.2 Approximating Functions in Robotics 38 smoothness, differentiability, and simplicity of parameterization. Also, spline theory shows that any smooth curve can be approximated to an arbitrary degree of accuracy using an adequate number of splines [18]. Moreover, a small number of splines is generally sufficient to approximate a smooth curve well. This philosophy was originally applied for determining time optimal paths. The path was parameterized in some appropriate manner and the algorithm in [5] was used to compute the minimum time motion along this path. Then, an optimization method suited for incorporating the specific constraints was used to find the optimal path by varying the path parameters. Rajan [46] assumed that the time optimal path should be smooth (sharp corners would require the manipulator to slow down) and that it could adequately be approximated by a low number of cubic splines. Hence, he parameterized the joint space path by one spline, discretized the parameter space and searched it to find the minimum. A gradient descent technique was applied afterwards to locate the minimum more precisely. The same type of approach was extended to path planning in cluttered environments by Gilbert and Johnson [25], [30], Dubowsky et al. [20], Shiller and Dubowsky [52], [53], and Bobrow [4]. Gilbert and Johnson [25], [30] used basis functions at least twice differentiable for path para-meterization. The path was assumed to be linear in the basis functions. Obstacle constraints were incorporated as special state constraints and the constrained optimization was transformed into an unconstrained one by use of penalty functions. The distance functions were shown to be continu-ously differentiable if parts were modeled by strictly convex sets and simple gradient formulas for them were derived. The optimal path was found through a Broyden-Fletcher-Goldfarb-Shanno-type algorithm that optimized the basis functions coefficients of an initially feasible path. Dubowsky et al. [20] studied different path parameterizations. Thus, straight lines connected by circular arcs, perturbations about a straight line by a Fourier Series and cubic Bezier splines were used for path representation. Among them, splines were found to allow easy manipulation of the path's shape through a limited number of parameters. Obstacle and joint constraints were incorporated into the cost through weighted penalties. This approach had the desirable feature of keeping the manipulator some distance away from the constraints. For solving the parameter optimization problem, both a Quasi-Newton method and the Pattern Search method were applied. 3.2 Approximating Functions in Robotics 39 A cubic B spline path parameterization in conjunction with the Pattern Search method was applied by Shiller and Dubowsky [52] to plan time optimal paths tangent to the Acceleration Lines. The same authors [53] extended then the technique to incorporate gripper and pay load constraints. They found that the Pattern Search method, which did not require derivatives of the cost function, performed better in the majority of the problems. Bobrow [4] used a similar approach for determining time optimal motions while incorporating joint limits, actuator torque limits, and collision avoidance constraints. He parameterized the path by cubic B splines, since B splines allow arbitrary smoothing of the path. The gradients of the objective function and the constraints were estimated numerically, and the vertices defining the path were varied till the minimum transversal time path was found. The problem was cast as a sequence of unconstrained minimizations with cubic interior penalty functions for the constraints. Each unconstrained minimization was solved by the Davidon-Fletcher-Powell (DFP) search technique. Croft et al. [16] determined near-time optimal paths as a step towards on-line time optimal motion planning. They used a cubic Hermite spline for path parameterization together with a two-step optimization algorithm. At the first level, analytical formulations and heuristic rules were used to select the best initial path direction and an initial path approximation. At the second level, the flexible polyhedron method (FPM) was employed to determine the optimal path parameters. The near-time optimal path was chosen as the minimum curvature one, with the intent of avoiding sharp corners. As the analysis indicated that the cost was a smooth function of the two variable parameters, a Quasi-Newton method (e.g., the DFP method) was indicated as an alternative minimization technique. Finally, at the path planning level, Martin and Bobrow [40] used cubic B splines for parame-terizing a minimum effort joint space path. The path was assumed to depend linearly on the path parameters and the Newton-Euler formulation of robot dynamics was expressed in terms of the product of exponentials formula. This formulation allowed the authors to compute analytic gra-dients of the objective function with respect to the path parameters and thus a steepest descent technique was applied to find the optimal path. Cubic [9], [28], [38] or quartic splines [9], [10] were also used for parameterizing joint space trajectories. However, in the case of trajectory planning, the TOC problem was not solved through nonlinear programming, but rather through scaling the trajectory subjected to joint, velocity, 3.3 Proposed Method 40 acceleration, and jerk limits. 3.3 P r o p o s e d M e t h o d Similar to the research reviewed in Section 3.2, the present work solves a TOC problem through a mathematical programming technique. That is, the SPCTOM trajectory is computed through a search in the s — s plane for the minimum time smooth trajectory. However, in this approach, rather than parameterizing the Cartesian path or the joint space path, the state space trajectory is parameterized. By doing so, the TOC problem is formulated as a fixed end points problem which lends itself to solving through nonlinear optimization. Nevertheless, in using a state space trajectory parameterization, the choices of basis functions and of trajectory parameters must be justified. 3.3.1 Basis Functions As shown in Section 3.1, the SPCTOM trajectory is a smooth curve in the s — s plane; i.e., it is C 2 continuous. Hence, the lowest degree polynomials that can be used to approximate it are cubics. Higher degree polynomials present no an advantage here, since the extra degrees of freedom they offer are not needed. Also, the optimal trajectory is obtained from a small number of splines. Therefore, the higher the polynomial degree of the splines, the more oscillatory they become. As a result, higher order polynomials will actually result in a poorer representation of the SPCTOM trajectory. In view of this, cubic polynomials are chosen as basis functions herein. The base trajectory is obtained by splining them together through selected knot points. The coefficients of the cubic polynomials are computed using the Matlab Spline Toolbox. The selection of the knot points, as well as other properties of the polynomials, are discussed in the following section. 3.3 Proposed Method 41 3.3.2 Optimization Variables The parameters defining the trajectory are the pseudo-velocities of the end-effector at predeter-mined points along the Cartesian path (knot points) and the slopes of the trajectory in the s — s plane at the path end points. Thus, the pseudo-velocities and the trajectory end slopes are the optimization variables in this problem. These variables control the motion time : the higher the knot points (as located in the phase plane), the shorter the trajectory transversal time. On the other hand, the end slopes control the speed at which the actuator torques leave or approach their static equilibrium values. Therefore, steeper slopes result in shorter motion time. The trajectories shown in Figure 3.2 1 illustrate the influence of the trajectory parameters on the motion time. The solid line represents an exemplar trajectory. The dot-dashed line is the trajectory obtained when the initial trajectory slope was increased by 70%; the dashed line was obtained by increasing the final trajectory slope by 70%; and the dotted line was obtained by increasing the third knot-point pseudo-velocity by 70%. path parameter [m] Figure 3.2: Sample trajectories in the s — s plane and their motion times. 1 A l l the figures in this chapter are plotted for the elbow manipulator with the dynamic parameters given in Table 2.1 moving along the parabolic path specified by Equations (2.1). 3.3 Proposed Method 42 In order to define the spline basis functions, the location of the knot points along the path must be specified. Fixing the location of the knot points along the Cartesian path results in lost degrees of freedom in the search for the optimal path. However, this is a necessary step in transforming the TOC problem into a tractable optimization. In this work, the location of the knot points is chosen to be the same as that of the PCTOM trajectory switching points (Figure 3.3). These switching points indicate where each joint actuator is, in turn, saturated due to the generalized dynamic forces of the robot's motion under PCTOM. Since PCTOM is the limit for SPCTOM, these switching points are, in the limit, the same for SPCTOM and provide a reasonable estimate of the location of the SPCTOM switching points along the parameterized path. Further work to improve these switching points is beyond the scope of this thesis. Extra knot points could be chosen; however, as the number of the PCTOM trajectory switching points can be considerable (five for the PCTOM trajectory in Figure 3.3), the addition of more knot points would significantly increase the number of optimization variables (from seven to twelve in this example). For simpler PCTOM trajectories, such as those having a single switching point, using more knot points is a reasonable option which is investigated in Chapter 4. 0.4 0.6 path parameter [m] Figure 3.3: Switching points of the PCTOM trajectory. 3.3 Proposed Method 43 The optimization variables are shown in Figure 3.4, for an exemplar splined trajectory. The knot points are indicated with the "o" symbol and the end slopes are marked with tangent lines at the beginning and end of the trajectory. The vector of optimization variables, x, is defined as: / ds 4A \ T \Tsm,0 A m > 1 Sm'P TsmjJ where the values with the index m correspond to the limiting PCTOM (the dashed line in Fig-ure 3.4), while the other values correspond to the splined trajectory (the solid line), and p is the number of switching points of the PCTOM trajectory. The optimization variables are normalized since the end slopes vary over a much wider range than the pseudo-velocities. The normalization is performed with respect to the PCTOM trajectory because this trajectory always represents an upper bound for the smooth optimal motion, namely, time optimal motion without smoothness constraints. PCTOM SPLINED TRAJ. VLC path parameter [m] Figure 3.4: The optimization variables. The knot-points are indicated with the "o" symbol. 3.3 Proposed Method 41 15, Figure 3.5: Motion time and its level curves with respect to several parameters varied over their ranges. The selection of an optimization method depends on the relationship between the identified optimization vector, x, and the cost function, namely, motion time. As is typically the case with trajectory optimization, the motion time is not linear in the variables. The relationship between the cost function and the parameters is illustrated in Figure 3.5, where the trajectory transversal time is plotted with respect to two of the variables over their range, namely from zero to one. Furthermore, there exist islands of inadmissibility in the parameter space. That is, there are points in the parameter space that result in trajectories that require negative pseudo-velocities. This phenomenon was first observed by Shin and McKay [55]. An example of such a case is shown in Figure 3.6. path parameter [m] Figure 3.6: Plot of a phase plane trajectory resulting from a point in the inadmissible region of the parameter space. The resulting trajectory is physically meaningless (cannot be achieved). 3.3 Proposed Method 45 Thus, the SPCTOM problem is recast as follows : the SPCTOM trajectory results from splining cubic polynomials in the s — s phase plane through a set of optimized x knot points. The knot point locations along the path are the same as the location of the switching points of the P C T O M trajectory. The end-effector pseudo-velocities at these locations and the trajectory slopes at the path end points are determined through an optimization process. The end-effector pseudo-velocities must all be positive, i.e., the trajectory must be admissible. The splined trajectory must be within actuator torque and actuator jerk limits and take minimum time. 3.3.3 Numerical Solution The above problem is solved by finding the optimal trajectory end slopes and the pseudo-velocities for the knot points in the parameter space for the splined trajectory. Therefore, the numerical solution of the SPCTOM trajectory planning problem is an approximation of the infinite dimensional OCP by a p + 2 dimensional optimization problem. The approximation is motivated by the complexity of the OCP. There are a number of considerations involved in such a numerical solution for an OCP, namely: • The incorporation of equality constraints, i.e. system dynamics. • The treatment of inequality constraints, i.e. actuator torque and jerk constraints. • The choice of a descent algorithm. • The different functions and their derivatives evaluation. • The determination of a good initial feasible approximate solution. • The achievement of a reasonable computational cost. Clearly, there is no algorithmic approach that best addresses these issues for any problem. There-fore, the method proposed herein is based on the particulars of the SPCTOM trajectory planning; namely: • The cost function and the constraints are nonlinear functions of the trajectory parameters. Furthermore, they are not analytic functions, but can only be computed numerically. 3.3 Proposed Method 46 • Objective function evaluations are expensive. • Analytical derivatives of the cost function and the constraints are not available. • The transversal time may not be differentiable with respect to the trajectory parameters, primarily because of the numerical approximations involved in its computation [53]. • The solution is expected to lie on the constraint boundary. Two options exist for solving a constrained optimization: (i) transformation of the constrained optimization problem into an unconstrained problem by incorporating the constraints into the ob-jective through penalties; (ii) directly solving the constrained optimization problem. Since a penalty function approach drives the solution away from the constraints, where the solution is expected to be found, a constrained optimization is more appropriate for planning SPCTOM trajectories. Furthermore, a method that does not require analytical or numerical derivatives is required. Fi-nally, a method that minimizes objective function evaluations is important due to the computation expense of such evaluations. In view of this, the optimization is carried out using the flexible tolerance method (FTM) in [27]. The F T M is a mathematical programming technique that improves the value of the objec-tive function by using information provided by both feasible points, as well as certain nonfeasible points termed near-feasible points (Figure 3.7). As the search proceeds, the near-feasibility limits (determined by the tolerance $) are made more restrictive, until in the limit only feasible points are accepted. The details of the F T M , as well as the implementation of the algorithm, are pre-sented in Appendix A. However, the F T M features that make it suited for determining SPCTOM trajectories are reviewed here. Namely: • It allows for nonlinear objective function and nonlinear equality and/or inequality constraints. • It transforms all equality and inequality constraints into one single inequality constraint, which results in a problem simplification. • It does not require derivatives. • It uses information about both feasible and near-feasible points, thus maintaining the search close to the constraint boundary. 3.3 Proposed Method 47 P' Figure 3.7: The parameter space in the Flexible Tolerance Method - Example for a two-dimensional optimization. • It is computationally economical, because it does not spend a considerable amount of com-putation effort in satisfying rigorous feasibility requirements. • It has successfully been applied to complex engineering problems with up to nine variables and forty-four constraints [15], [22]. As other constrained optimization techniques, the F T M requires an initial guess of a feasible solution. However, guessing such a solution is not difficult. If the path is feasible (namely, within the static and kinematic robot workspace), then there always exists a feasible trajectory (although it could be infinitely slow). The question of whether the search has returned the global optimum has no definite answer. This is the case with any nonlinear optimization. In this case the objective function, i.e. motion time, is a complex function of many variables and it appears to have many local minima. To increase the confidence that the global optimum has been achieved, it is proposed herein to rerun the optimization starting with a number of different initial guesses. 3.4 Summary 48 3.4 S u m m a r y This chapter developed an optimization solution for the SPCTOM trajectory planning problem formulated in Chapter 2. The method proposed is based on solving an infinite dimensional OCP by a finite dimensional optimization. This approach uses cubic polynomials splined together through knot points to approximate the SPCTOM trajectory in the s — s plane. Cubic splines have been used in robotics before for approximating optimal paths or optimal trajectories in the time domain. However, this is the first time that they have been used for trajectory interpolation in the phase plane. The knot point location along the Cartesian path is chosen to be the same as the location of the switching points of the PCTOM trajectory. This provides a compromise between an acceptable number of optimization variables and the dynamic characteristics of the robot along the given path. The computation of the optimal trajectory reduces to a search for the optimal knot points in the parameter space. The search is performed using the F T M since it allows for nonlinear cost function, as well as equality and/or inequality constraints. Furthermore, the algorithm has low requirements on an initial feasible trajectory and can find optimal solutions that are on the constraint boundary. As with any other nonlinear optimization algorithm, no guarantee exists that the global optimal trajectory has been determined. Therefore, the search for the optimal parameters is performed multiple times, each time starting from another point in the parameter space. The trajectory with minimum transversal time is then chosen as the computed SPCTOM trajectory. Chapter 4 Simulations In this chapter, SPCTOM trajectories are planned using the methodology developed in Chap-ter 3. As well, the effect of selecting more knot points than the number of switching points for the related PCTOM trajectory is investigated. A planning level comparison is performed between the SPCTOM trajectory, the PCTOM trajectory and a standard smooth polynomial trajectory. Finally, the performance of each of these trajectories is compared using a simulated robot and controller model. The simulations presented herein serve several purposes: (i) they demonstrate the methodology developed in Chapter 3; (ii) they allow the assessment of whether or not the proposed method is suitable for computing SPCTOM trajectories; (iii) they establish a correlation between the limits imposed on the actuator jerks and the quality of the trajectory (as reflected by the tracking accu-racy); and (iv) they permit an evaluation of the performance of a SPCTOM trajectory compared to other trajectories. The trajectory performance is evaluated from the point of view of both improving task produc-tivity and reducing the strain on the robot and the controller. Thus: • Motion time reflects task productivity. • Tracking accuracy determines the quality of the trajectory. • Actuator torques smoothness reflects the robot and actuator strain. 49 4.1 SPCTOM Planning 50 4.1 S P C T O M Planning In this section, the methodology developed in Chapter 3 is demonstrated through simulations. The effect of choosing more knot points than the number of switching points of the related P C T O M trajectory is studied. The robot considered throughout this section is the elbow manipulator introduced in Chapter 2. Again, the robot dynamic parameters, the torque limits, and the two exemplar paths are taken from [45]. The robot data is repeated here for convenience (Tables 4.1 and 4.2). The dynamic model of the robot is detailed in Appendix B. The three different jerk limits have been selected to demonstrate the planning results. The "low jerk limits" represent a very smooth motion with low stress on the manipulator. The "high jerk limits" represent the other end of the range, approaching P C T O M motion in the limit. The "medium jerk limits" fill out the range. Table 4.1: Robot parameters (repeated). Link [m] C.O.M. [m] Mass [kg] Ix[kgm2] Iy[kgm2] Iz[kgm2] h = o Id = 0.05 mi = 0 1x1=0 lyl = 5 hi = 0 l2 = 0.75 lc2 = 0.2 7712 = 6.6 1x2 = 0.1 Iy2 = -6 Iz2 = 0.6 h = 0.75 la = 0-15 m 3 = 4.2 Ixs = 0.02 Iy3 = -2 Iz3 = 0.3 Table 4.2: Actuator parameters (repeated). Torque limits T[Nm] Medium jerk limits Example 1 fexl[Nm/sec] High jerk limits Example 2 fex2[Nm/sec] Low jerk limits Example 3 fex3[Nm/sec] Ti = 140 r 2 = 140 T 3 = 50 f n = 500 T 2 i - 500 T 3 i = 100 f 12 = 5000 T22 = 2000 T32 = 1000 Tis = 140 T 2 3 = 140 T33 = 50 4.1.1 Parabolic Path In the following examples, the end-effector moves along the parabolic path given by Equa-4.1 SPCTOM Planning 51 tions (2.1), namely: x(s) = 0.5 y(s) = 20s3 - 30s2 + 10s z(s) = s-0.5 s = 0,... ,1. The PCTOM trajectory has five switching points (Figure 3.3). The location of these points in the s — s plane is given in Table 4.3. The slopes of the PCTOM trajectory in this plane are 19.875 at the starting point and —24.07 at the ending point. As explained in Chapter 3, these values are used for scaling the SPCTOM variables. Table 4.3: Location of the PCTOM switching points in the s — s plane. Switching point 1 2 3 4 5 Path parameter si = 0.036 s2 = 0.216 s3 = 0.492 s4 = 0.752 s5 = 0.956 Pseudo-velocity si = 0.344 S2 = 0.932 s3 = 0.697 s4 = 0.955 s5 = 0.497 Hence, in this example, the vector of optimization variables for SPCTOM is: ( ds . . . . . ds -dip si s2 s3 s4 s5 ds f 19.875 0.344 0.932 0.697 0.955 0.497 -24.07 The SPCTOM trajectories are determined for the three different jerk limits in Table 4.2. These trajectories are obtained through a nonlinear programming method. The cost function, namely, the motion time, is multi-modal. Therefore, it is not possible to decide if the global optimum has been determined. To improve the chance of determining the global minimum, four different initial guesses are used. In the end, the optimization is restarted using three of these optima (the best one is excluded) as initial guess values. The SPCTOM trajectory is computed by splining cubics through the optimal knots obtained in the optimization. The motion times obtained after each optimization, as well as after the restart, are shown in Table 4.4. The PCTOM trajectory and the SPCTOM trajectories for the three examples (i.e. the fastest 4.1 SPCTOM Planning 52 trajectory in each case), together with the corresponding actuator torques and jerks, are plotted in Figures 4.1-4.4. Table 4.4: Optimal motion times for the elbow manipulator along the parabolic path [sec]. Initial guess SPCTOM PCTOM Example 1 Medium jerk limits Example 2 High jerk limits Example3 Low jerk limits 1 *i = 3.03 t2 = 1.91 t3 = 4.08 2 h = 3.45 t2 = 2.05 t3 = 4.24 3 *i = 3.85 t2 = 2.61 t3 = 5.07 t = 1.72 4 *i = 2.80 t2 = 2.06 t3 = 4.97 Restart ti = 2.74 t2 = 1.91 t3 = 3.93 I'21 8 So.e| 1° ° 0 . 4 0.2 0 VLC PCTOM 0.4 0.6 0.8 path parameter [m] (a) P C T O M trajectory in the s-s plane. Actuator 1 Actuator 2 Actuator 3 path parameter [m] (b) Actuator torques. 1-2; Actuator 1 Actuator 2 Actuator 3 0.4 0.6 0.8 path parameter [m] (c) Actuator jerks. Figure 4.1: PCTOM for the elbow manipulator along the parabolic path. As shown in Table 4.4, by restarting the optimization either the previous best trajectory is obtained or a new trajectory with a lower motion time is found. No guarantees exist that the trajectory obtained after the restart is the globally optimal one. However, if, after the restart, no or very little improvement is obtained, the confidence that the global optimum has been achieved increases. As this is the case with the three examples presented here, it is expected that the trajectories found approach the SPCTOM trajectory in each case. The second example (Figure 4.3) further supports the confidence that the global optima have been obtained. In this example, the actuator jerk limits are very high. Hence, the trajectory is determined by the limits on the actuator torques and the increase in motion time is very small 4.1 SPCTOM Planning 53 SMVLC — PCTOM SPCTOM •" / V / \°' .-' 1 / 0.4 0.6 0.8 path parameter [m] 2500 2000 n-1000 -1500 -2000 -2500, Actuator 1 Actuator 2 Actuator 3 path parameter [m] path parameter [m] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.2: SPCTOM for the elbow manipulator along the parabolic path - Example 1 (medium jerk limits). path parameter [m] path parameter [m] path parameter [m] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.3: SPCTOM for the elbow manipulator along the parabolic path - Example 2 (large jerk limits). SMVLC — PCTOM SPCTOM \ ~ % '' / \ ? \ y- S ' / — Actuator 1 Actuator 2 Actuator 3 path parameter [m] 0.2 0.4 0.6 0.8 path parameter [ml 2500 2000 1500 • 1000 . 500 0 -500 -1000 -1500 -2000 -2500 Actuator 1 Actuator 2 Actuator 3 path parameter fm] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.4: SPCTOM for the elbow manipulator along the parabolic path - Example 3 (low jerk limits). 4.1 SPCTOM Planning 54 compared to the PCTOM: 1.91 seconds compared to 1.72 seconds. Although one would expect both trajectories to yield the same motion times, there are two reasons for the increase in motion time for the SPCTOM: (i) the limitations of the parameterization chosen in the s — s phase plane and (ii) the significant decrease in peak actuator jerks for SPCTOM (solid lines) compared to P C T O M (dotted lines), as shown in Figure 4.5. The logarithmic pseudo-jerk scale in Figure 4.5 shows that the peak P C T O M jerk is almost two orders of magnitude greater than the peak SPCTOM jerk. 10 s 10 s 10* O TP , l 1 0 CL 10' 10° 10"' 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 path parameter s Figure 4.5: Actuator jerks for the SPCTOM in Example 2 (solid lines) and the P C T O M (dotted lines) for the elbow manipulator along the parabolic path. 4.1.2 Circular Path In this section, a second example is presented. This second example is used for two purposes: (i) to illustrate a different path; and (ii) to investigate the influence of the number of knot points on the results of the optimization. The robot is the same elbow manipulator as before, but the path is chosen to be a circular path 4.1 SPCTOM Planning 55 given in parametric form as: x(a) = 0.85 - 0.65 • cos(5.5s + 0.4) 0.65 -sin(b.bs + 0.4) (4.2) z(s) = 0.1 s = 0...1. Again, the path is taken from [45], but is scaled such that the path parameter varies between 0 and 1. The PCTOM in this example is shown in Figure 4.6. As shown in the figure, there is only one switching point. That is, the minimum time motion is bang-bang: accelerating with maximum allowable acceleration and then decelerating with maximum allowable deceleration. The transversal time of the PCTOM trajectory is t = 1.07 sec. Following the methodology proposed in Chapter 3, the base trajectory representing the SPCTOM consists of two cubics splined together at s = 0.496 along the Cartesian path and the vector of optimization variables is: The SPTOM trajectories are determined for all three actuator jerk limits in Table 2.2 following the procedure outlined in the previous section. Namely, for each set of jerk limits, the optimization is started with four different initial guesses. The best trajectory is discarded and the other three are used to restart the process. The results are given in Table 4.5. The PCTOM trajectory, the SPCTOM trajectories, and the corresponding optimal actuator torques and jerks, are plotted in Figures 4.7-4.10. As Table 4.5 shows, the increase in motion time for the SPCTOM trajectory as compared to the PCTOM trajectory is significant. There are two factors that result in a SPCTOM that is slower than the PCTOM: (i) the SPCTOM trajectory parameterization; and (ii) the substantially lower peak actuator jerks for SPCTOM (Figure 4.11). (4.3) 4.1 SPCTOM Planning 56 3 . 5 3 h , 2 . 5 1 . 5 0 . 5 h ' VLC PCTOM 0 . 1 0 . 2 0 . 3 0 . 4 0 . 5 0 . 6 0 . 7 0 . 8 0 . 9 1 path parameter [m] Figure 4.6: P C T O M in the s — s phase plane for the elbow manipulator along the circular path. Table 4.5: Optimal motion times for the elbow manipulator along the circular path [sec]. Initial guess SPCTOM P C T O M Medium jerk limits High jerk limits Low jerk limits Example 1 Example 2 Example 3 1 ti = 2.73 t2 = 1.56 t3 = 3.31 2 *i=2.14 t2 = 1-57 t3 = 2.91 3 tx = 2.18 t2 = 1.57 t3 = 3.05 t = 1.07 4 ti = 2.26 t2 = 1.59 t3 = 2.90 Restart * i = 2.13 t2 = 1.57 t3 ='2.91 | 0 . > 10.6 o =•0.4 0.2 0 VLC PCTOM 0.4 0.6 path parameter [mj (a) P C T O M trajectory in the s-plane. 1.5-iV • ' x J I 1-E z I 0.5-o actua \ — Actuator 1 — Actuator 2 Actuator 3 -0.5 ) 0.2 0.4 0.6 0.8 I 0 path parameter [m] (b) Actuator torques. Actuator 1 — Actuator 2 Actuator 3 0.2 0.4 0.6 path parameter [m] (c) Actuator jerks. Figure 4.7: PCTOM for the elbow manipulator along the circular path. 4.1 SPCTOM Planning 57 SMVLC PCTOM SPCTOM — 400 E S. 200 ' v " —3c ' J Actuator 1 Actuator 2 Actuator 3 path parameter [m] path parameter [m] path parameter [m] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.8: SPCTOM for the elbow manipulator along the circular path - Example 1 (medium jerk limits). path parameter [m] path parameter [m] path parameter [m] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.9: SPCTOM for the elbow manipulator along the circular path - Example 2 (high jerk limits). SMVLC PCTOM SPCTOM path parameter [m] Actuator 1 Actuator 2 Actuator 3 path parameter [m] 600 •5T 400 E z 200 Actuator 1 Actuator 2 Actuator 3 path parameter [m] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.10: SPCTOM for the elbow manipulator along the circular path - Example 3 (low jerk limits). 4.1 SPCTOM Planning 58 10 r-10s r 10' 1 0 - l , , , , , , , , , 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 path parameter s Figure 4.11: Peak actuator jerks for the SPCTOM (solid lines) and the P C T O M (dotted lines) for the elbow manipulator along the circular path. Since the SPCTOM trajectory is represented by only two cubics splined together, one can consider whether the trajectory parameterization is adequate. To resolve this issue, two extra knot points are chosen, located at equal distances from the path end points and the switching point. Now, the optimal trajectory is approximated by four cubics and the vector of optimization variables is: / ds • • • ds \ T _ / J s 0 _ _ J i _ J i _ J ± _ d s f \ . , \^52.7 1.127 1.48 1.31 -53.07J v ' ' The results obtained using this trajectory parameterization are given in Table 4.6. The SPCTOM trajectories for all three actuator jerk limits are plotted in Figures 4.12-4.14. Table 4.6: Optimal motion times for the elbow manipulator along the circular path when additional knot points are inserted [sec]. Initial guess SPCTOM P C T O M Medium jerk limits High jerk limits Low jerk limits Example 1 Example 2 Example 3 1 tx = 1.97 t2 = 1.46 t 3 = 2.83 2 tx = 2.00 * 2 = 1-33 t 3 = 2.92 3 * i = 2.02 t2 = 1.30 t 3 = 2.92 t = 1.07 4 t i = 2.15 t2 = 1.33 h = 2.73 Restart h = 1.97 * 2 = 1-29 * 3 = 2.73 4.1 SPCTOM Planning 59 (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.12: SPCTOM for the elbow manipulator along the circular path (additional knot points inserted) - Example 1 (medium jerk limits). V'-• SMVLC - - - PCTOM SPCTOM 100 I 50 I ° o (3 I -50 X v Actuator 1 — Actuator 2 Actuator 3 Actuator 1 — Actuator 2 Actuator 3 path parameter [m] path parameter [m] path parameter [m] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.13: SPCTOM for the elbow manipulator along the circular path (additional knot points inserted) - Example 2 (high jerk limits). — 1 3L„ SMVLC PCTOM SPCTOM 100 | 50 §" Q O ID S -501 — Actuator 1 — Actuator 2 Actuator 3 path parameter [m] 0.4 0.6 path parameter [m] Actuator 1 Actuator 2 Actuator 3 0.4 0.6 0.8 path parameter [m] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.14: SPCTOM for the elbow manipulator along the circular path (additional knot points inserted) - Example 3 (low jerk limits). 4.2 Discussion 60 The results in Table 4.6 show that the SPCTOM trajectory is better parameterized by more that two cubics. In view of this, it is recommended that at least three knot points are used when computing the SPCTOM. By doing so, the limitations involved in the trajectory parameterization are reduced while the increase in the computational cost is negligible. 4.2 D i s c u s s i o n Motion times for both the PCTOM and the SPCTOM for all three examples presented in Sections 4.1.1 and 4.1.2 are summarized in Table 4.7. Table 4.7: P C T O M and SPCTOM motion times for the examples in Sections 4.1.1 and 4.1.2 [sec]. Parabolic path Circular path P C T O M SPCTOM PCTOM SPCTOM medium 1 high 2 low 3 One knot Three knots medium 1 high 2 low 3 medium 1 high 2 low 3 1.72 2.74 1.91 3.92 1.07 2.13 1.57 2.90 1.97 1.29 2.72 From the results from Table 4.7 and other simulation examples run during the course of this research, the following conclusions can be drawn: • The SPCTOM trajectory parameterization is adequate when knot point locations are the same as the PCTOM switching point locations if the P C T O M has more than one switching point. If the PCTOM has only one switching point, using two additional knot points (equally spaced between the path end points and the switching point) results in an improved trajectory approximation. • Bounding the actuator jerks yields increased motion time for the SPCTOM compared to the PCTOM. The more restrictive the bounds are, the more significant the increase in motion time is. • If the actuator jerk bounds are set sufficiently high, then the actuator torques become the active constraints (as in the case of second set of bounds for the robot along the parabolic 4.3 SPTOM Performance Investigation 61 path, Figure 4.3(b)- 4.3(c)). The slight increase in motion time is then due to the smoothing of the actuator torques. 4.3 S P T O M Performance Investigation The influence of the actuator jerk bounds on the trajectory traversal time has been analyzed in the previous section. Herein, their influence on the controller tracking performance is investigated through simulations. As shown in the previous section, from a planning level perspective, the smoothness of the trajectory has a negative impact on the motion time. That is, the higher the actuator jerks are, the lower the motion time is, with the P C T O M trajectory being the fastest possible motion given the limits on the actuator torques. These planning level results do not, however, give an indication of the system performance, i.e., the tracking accuracy. Thus, it is not possible to decide how to strike a balance between these conflicting requirements: namely, increased task productivity, as reflected in the transversal time, and good system performance, as reflected by the tracking accuracy. The following simulations address this issue. Since the experiments reported in the following Chapter 5 are carried out on the SCORBOT ER VII robot in the Industrial Automation Laboratory (IAL) at UBC, the simulations performed herein use the same manipulator. Its model is built in Simulink using estimated dynamic parameters. The SCORBOT ER VII Denavit-Hartenberg parameters are given in Table 4.8, while estimated link masses and inertias are given in Table 4.9. Only the robot positional degrees of freedom are considered. Hence, for the purpose of the simulations performed here, the SCORBOT ER VII is considered a 3-dof elbow manipulator. The detailed dynamic model of the robot is presented in Appendix B. Table 4.8: The SCORBOT ER VII Denavit-Hartenberg parameters. Link 0[rad] d[m] a[m] a[rad] 1 01 =0 di = 0.3585 oi = 0.050 a i = - f 2 02 =0 d 2 = -0.037 a 2 = 0.300 « 2 = 0 3 03 = 0 d3 = 0.0 a 3 = 0.250 a 3 = 0 No parameter identification has been performed because the high friction in the joints negates 4.3 SPTOM Performance Investigation 62 Table 4.9: SCORBOT ER VII estimated masses and inertias. Mass [kg] I x [ k g m 2 ] I y [ k g m 2 ] h [ k g m 2 ] m \ = 0 h i = o . o o l y l = 0.05 h i = 0.00 TT12 — 6.6 1x2 = 0.10 Iy2 = 0.60 J* 2 = 0.60 7713 = 4.2 h i = 0.02 I y 3 = 0-20 I z s = 0.30 Table 4.10: Actuator torque and jerk bounds for the SCORBOT ER VII. Torque limits T[Nm] Medium jerk limits Example 1 fi[Nm/sec] High jerk limits Example 2 f2[Nm/sec] Low jerk limits Example 3 f3[Nm/sec] Ti = 10 T 2 = 10 T 3 = 10 Tn = 100 T2l = 100 T 3 i = 100 T12 = 1000 T 2 2 = 1000 T 3 2 = 1000 T i 3 = 10 T 2 3 = 10 T 3 3 - 10 the possibility of obtaining accurate results (as determined by previous attempts1). Friction itself has been modelled as Coulomb and viscous friction, with the Coulomb friction coefficients 2.0[Nm] and the viscous friction coefficients 0.2Nmsec for all three links. While an accurate robot model would yield better results, it is seldom the case in industrial practice that such a model is available. Therefore, a comparison between the tracking accuracy for the P C T O M and the SPCTOM even with a poor robot model is considered to have relevance. The actuator torque limits were computed using data in the actuator manufacturer catalogue. As before, the three sets of actuator jerk limits were set arbitrarily, Table 4.10. The path is a straight line given in parametric form as: x(s) = 0.4 y(s) = 0.35-0.1 z{s) = 0.2s+ 0.3 (4.5) s = 0,... ,1. Since the P C T O M of the SCORBOT ER VII along this path has a single switching point (Fig-ure 4.15), two additional knot points are used for parameterizing the SPCTOM trajectory. Along the path, the three knot points are located at s = 0.25, s = 0.49, and s = 0.75 respectively. The 1S. Gu, UBC IAL, Personal communication 4.3 SPTOM Performance Investigation 63 14 12 ,10 VLC PCTOM 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 path parameter [m] Figure 4.15: PCTOM trajectory in the s - s phase plane for the SCORBOT ER VII along the linear path. vector of optimization variables is: x = dsO Si Si Si 41.86 2.173 3.362 2.582 ds dsf -55.65 (4.6) A fifth trajectory used in simulations is a standard quintic trajectory. The motion time for the quintic trajectory is given by [2]: tq — uis,x{tv,tay, (4.7) where: Ihd 8Un tv — ta = A/5.77 (4.8) (4.9) with d being the distance traveled by the end-effector during the motion and vmax and amax the (estimated) maximum end-effector velocity and acceleration. Once the motion time has been 4.3 SPTOM Performance Investigation 64 determined, the quintic trajectory is found in parametric form by solving for the coefficients in: s(t) = a5t5 + a4t4 + a 3 i 3 + a2t2 + ait + a 0, (4.10) by applying the boundary conditions: s(0) = 0, HO) = o , 5(0) = 0, (4.11) s(tq) = 1, s(tq) = 0, S{tq) = 0. The joint trajectories are then obtained by applying the inverse kinematics of the SCORBOT ER VII. The motion times for the PCTOM trajectory, the SPCTOM trajectories corresponding to the different jerk limits, and the quintic trajectory are given in Table 4.11. In Figures 4.16-4.20, all these trajectories are plotted in the s — s phase plane together with the corresponding actuator torques and jerks. Table 4.11: PCTOM, SPCTOM, and quintic trajectory motion times for the SCORBOT ER VII along the straight line path [sec]. PCTOM SPCTOM Quintic 1 2 3 0.59 0.735 0.7 1.5 2.0 A PID independent joint controller with position feedback is used to compare the tracking performance for these trajectories. It has been tuned for each link individually [36] for critical damping and a rise time of 200msec. The gains of the corresponding discrete time controller have been then determined for a sampling frequency of 200Hz. In the simulations, actuator torques saturate at lONm, which is the torque limit considered during planning. 4.3 SPTOM Performance Investigation 65 VLC PCTOM 6 E" 4 S 2 I ° 1 "2 Actuator 1 Actuator 2 Actuator 3 path parameter [mj path parameter [m] (a) P C T O M trajectory in the s s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.16: PCTOM for the SCORBOT ER VII along the linear path. SMVLC PCTOM SPCTOM path parameter [m] (a) S P C T O M trajectory in the s — s plane. S * 3 1 S 1 -2| 3 Actuator 1 . Actuator 2 Actuator 3 0.2 0.4 0.6 0.8 path parameter [m] (b) Actuator torques. -200 -300 -400 path parameter [m] Actuator 1 Actuator 2 Actuator 3 (c) Actuator jerks. Figure 4.17: SPCTOM for the SCORBOT ER VII along the linear path (additional knot points inserted) - Example 1 (medium jerk limits). path parameter [m] path parameter [m] path parameter [m] (a) S P C T O M trajectory in the (b) Actuator torques. (c) Actuator jerks. s — s plane. Figure 4.18: SPCTOM for the SCORBOT ER VII along the linear path (additional knot points inserted) - Example 2 (high jerk limits). 4.3 SPTOM Performance Investigation 66 SMVLC PCTOM SPCTOM E 4 8 2i f ° 1 Actuator 1 Actuator 2 Actuator 3 -200 - 3 0 0 - 4 0 0 Actuator 1 Actuator 2 Actuator 3 path parameter [ml path parameter [m] path parameter [m] (a) S P C T O M trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.19: SPCTOM for the SCORBOT ER VII along the linear path (additional knot points inserted) - Example 3 (low jerk limits). SMVLC PCTOM Quintic 1 -2 | — Actuator 1 — Actuator 2 Actuator 3 0.4 0.6 0.8 path parameter [m] 400 300 — 200 I 100 I 0 S H O O " -200 -300 -400 Actuator 1 Actuator 2 Actuator 3 path parameter [m] path parameter (m] (a) Quintic trajectory in the s — s plane. (b) Actuator torques. (c) Actuator jerks. Figure 4.20: Quintic trajectory for the SCORBOT ER VII along the linear path. The tracking performance of the PID controller for all five trajectories is plotted in Figure 4.21, while the planned and simulated actuator torques are plotted in Figures 4.22-4.26. As seen in Figure 4.21, due to actuator torque saturation, the controller cannot keep the end-effector on the path when the actuator jerks are too high. This is the case with the PCTOM trajectory and the SPCTOM trajectories corresponding to actuator jerk limits of lOONm/sec and lOOONm/sec (labeled 'spctoml' and 'spctom2', respectively, in Figure 4.21). This result shows that actuator jerk limits are extremely important for the ability of the system to track a planned trajectory, especially in the face of poorly modelled system dynamics. As expected, the smoother the trajectory, i.e., the lower the actuator jerk limits, the higher the tracking accuracy of the controller. 4.3 SPTOM Performance Investigation 67 0.25 0.2 0.15 0.1 0.05 0 -0 .05 desired pctom quintic spctomi spctom2 spctom3 -°0 1 385 0.39 0.395 0.4 x axis [m] (Cartesian space) 0.5 0.48 _ 0 . 4 6 <D § 0 . 4 4 U> 10.42 (/> 0) S 0.4 o g-0.38 £ 0 . 3 6 CD N 0 . 3 4 0.32 0.3. '-, * \ I •A i W — desired i; l": pctom quintic — spctomi — - - spctom2 — spctom3 i i 0.405 0.385 0.39 0.395 0.4 x axis [m] (Cartesian space) 0.405 (a) Paths in the xOy plane. (b) Paths in the xOz plane. Figure 4.21: Controller tracking performance for the PCTOM, quintic, and SPCTOM trajectories. _ 10 E z C J o "-10 I 1 1 1 1 1 1 1 \ I : 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Figure 4.22: Desired and simulated torques for the PCTOM trajectory. 4.3 SPTOM Performance Investigation 68 10 0 -10 I 10 0 -10 I / 1 • • \ . . J ^ N ^ ^ > • planned simulated 1 1 I I 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.3 0.4 0.5 time [sec] Figure 4.23: Desired and simulated torques for the SPCTOM trajectory - Example 1 (medium jerk limits). D" CM E z CO CD =J cr 10 0 -10 ( 10 0 -10 ( 10 0 -10 0.1 1 1 1 - planned x ^ — ~ — 1 simulated \ 1 ' — . . . / 0.2 0.3 0.4 0.5 0.6 0.3 0.4 time [sec] Figure 4.24: Desired and simulated torques for the SPCTOM trajectory - Example 2 (high jerk limits). 4.3 SPTOM Performance Investigation 69 10 tu 0 3 <T i_ O "-10 planned simulated _ i i 0.5 1.5 time [sec] Figure 4.25: Desired and simulated torques for the SPCTOM trajectory - Example 3 (low jerk limits). CD 3 10 -10 CM CD 3 CT i O CO CD 3 CT O 1 1 1 1 1 1 • planned simulated 1 1 1 p 1 1 1 1 1 -0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 10 i I o I r --f 10 I 1 l i , I I -0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 10 1 1 1 I I i I I 0 -10 1 i 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 time [sec] Figure 4.26: Desired and simulated torques for the quintic trajectory. 4.4 Summary 70 Comparing the SPCTOM trajectory to the PCTOM and the quintic trajectories, the following conclusions can be drawn: • While the PCTOM trajectory represents the theoretical optimal motion when actuator torque limits are imposed, it is not feasible for direct implementation on a poorly modelled system with an unsophisticated controller, since it cannot be tracked by such a controller. On the other hand, the SPCTOM trajectory, while maintaining a measure of optimality, can also be tracked by the controller. Thus, it represents a feasible time-optimal motion. • Evaluated against the quintic trajectory, the SPCTOM trajectory yields an improved motion time for comparable controller tracking performance. This shows that fixed actuator jerk limits are a much better way of defining optimal smooth trajectories over quintics with fixed velocity and acceleration limits. The reason for this is that jerk limits are actuator related and position independent, whereas the velocity and acceleration capabilities of the robot are configuration dependent, thus the limits imposed on them are worst case limits. • Actuator jerks also represent a measure of the actuator torque smoothness. The lower the jerks are, the smoother the torques become. Thus, based on the relationship between torque smoothness and robot and actuator strain, [8], lower jerks also result in decreased system strain and wear. 4.4 Summary In this chapter, two objectives were pursued. First, the methodology proposed in Chapter 3 for planning an SPCTOM trajectory was demonstrated. Second, the performance of the SPCTOM trajectory compared to the PCTOM and to a quintic trajectory was investigated. The examples presented in Section 4.1 show that the FTM is adequate for planning SPCTOM trajectories. This conclusion is drawn from the fact that there is only a slight increase in the motion time of the SPCTOM with high jerk limits compared to the motion time of the PCTOM. The increase is attributed to the chosen trajectory parameterization in the phase plane, as well as to the decrease in actuator jerks. Thus, it is expected that the FTM has converged to near the 4.4 Summary 71 global optimum for the SPCTOM and that the methodology proposed in Chapter 3 is suitable for planning SPCTOM trajectories. The limitations involved in selecting a number of knot points equal to the number of switching points of the related P C T O M trajectory were investigated in Section 4.1 through an example. The example demonstrates that the parameterization of the SPCTOM trajectory in the phase plane by only two cubics is inadequate. Therefore, it is proposed that two additional knot points be inserted, since the added computational burden is negligible. In section 4.3, the performance of the SPCTOM trajectory is compared to that of the P C T O M and quintic trajectories through simulations. These simulations show that, while limited actuator jerks have a negative influence on motion time, they need to be imposed in order to ensure the required tracking accuracy when the available system model is poor. While sophisticated controllers might track the P C T O M trajectory of an accurately identified system, it is shown that in this case the PID independent joint controller often used in industrial practice cannot keep the end-effector of an approximately modelled robot along the path. Practically, it is difficult to obtain an accurate manipulator model and most systems are controlled by PID controllers. Therefore, imposing actuator jerk limits and planning SPCTOM trajectories accordingly results in the planning of feasible time optimal motions. Actuator jerk limits not only make the SPCTOM trajectories readily implementable on a ma-nipulator, they are also a better way of defining the optimal trajectory smoothness over quintics with global velocity and acceleration limits, because they are actuator dependent and configuration independent. In conclusion, the SPCTOM trajectory represents a balance between the conflicting require-ments of minimum motion time and specified tracking accuracy: it results in lower motion time compared to a quintic trajectory with similar tracking performance and, though slower that the P C T O M trajectory, it can be tracked by a typical industrial controller. Chapter 5 Experiments In chapter 4, the SPCTOM trajectory was compared with the PCTOM and quintic trajectories through simulations. In this chapter, the same comparison is done through experiments performed on the SCORBOT ER VII manipulator in the IAL at UBC. As previously stated, the view taken in this work and justified by simulations is that a desired degree of trajectory smoothness, which directly reflects upon system tracking performance, can be imposed through limits on the actuator jerks. This smoothness can compensate for modelling inaccuracies and thus feasible time optimal trajectories can be determined. These are trajectories that are readily implementable in an industrial setting, i.e., on a manipulator for which an accurate model is difficult to obtain and which is controlled by a PID independent joint controller. The goal of the present chapter is to provide the experimental proof of this assertion, while investigating appropriate jerk limits for practical applications. 5.1 Experimental Setup The experimental setup used to test the different trajectories is a. SCORBOT ER VII 5-dof robot in the IAL at UBC (Figure 5.1). The robot is controlled by a TMS320C32 DSP board, interfaced with two axis control cards, each capable of handling three axes simultaneously. An open architecture real-time operating system (ORTS) [21] is used in the implementation of the control algorithm and in reading the pre-planned trajectories and feeding them to the control loop 72 5.2 Experimental Results 73 at the controller frequency. The axis control cards and the real-time operating system ORTS were developed in-house by the Manufacturing Automation Laboratory, UBC. For the purpose of the experiments reported here, only the positional degrees of freedom of the robot are considered, thus the robot is treated as a 3-dof elbow manipulator with the DH parameters given in Table 4.8. A tuned discrete PID algorithm is used to provide the control law. This setup simulates typical condi-tions in industry, where the robot is equipped with a closed architecture discrete PID independent joint controller. Figure 5.1: Experimental setup. 5.2 E x p e r i m e n t a l R e s u l t s The PCTOM, SPCTOM, and quintic trajectories were all implemented on the SCORBOT ER VII and the encoder readings, as well as the actuator control voltages and output currents were recorded. The results of the experiments are plotted in Figures 5.2-5.6, and summarized in Table 5.1. 5.2 Experimental Results 74 Axis 1 - Control Performance A x ' s ^ - Control Performance Axis 3 - Control Performance „ 0.4 ~ 0.2 " % 0.5 1 1.5 2 0 0.5 1 1.5 2 0 0.5 1 1.5 2 time [sec] time [secj time [sec] (a) Joint 1 performance (b) Joint 2 performance (c) Joint 3 performance Figure 5.2: Experimental results for the PCTOM trajectory implemented on the SCORBOT ER VII. (a) Joint 1 performance (b) Joint 2 performance (c) Joint 3 performance Figure 5.3: Experimental results for the SPCTOM trajectory (Example 1 - jerk limits of lOONm/sec) implemented on the SCORBOT ER VII. (a) Joint 1 performance (b) Joint 2 performance (c) Joint 3 performance Figure 5.4: Experimental results for the SPCTOM trajectory (Example 2 - jerk limits of lOOONm/sec) implemented on the SCORBOT ER VII. 5.2 Experimental Results 75 (a) Joint 1 performance (b) Joint 2 performance (c) Joint 3 performance Figure 5.5: Experimental results for the SPCTOM trajectory (Example 3 - jerk limits of lONm/sec) implemented on the SCORBOT ER VII. Axis 1 - Conlrol Performance A x i s 2 " C o n t r o 1 Performance Axis 3 - Control Performance time [sec] time (secj time [sec] (a) Joint 1 performance (b) Joint 2 performance (c) Joint 3 performance Figure 5.6: Experimental results for the quintic trajectory implemented on the SCORBOT ER VII. Table 5.1: Experimental results for the PCTOM, SPCTOM, and quintic trajectories. Trajectory Jerk Motion Maximum RMS RMS RMS limits time tracking tracking tracking tracking [Nm/sec] [sec] error error 1 error 2 error 3 [mm] [1 [°] [°] P C T O M oo 4.0 13.0 17.5 3.1 15.8 SPCTOM 1 1000 4.0 13.4 17.2 2.8 14.8 SPCTOM 2 100 4.0 12.5 15.9 2.6 14.8 SPCTOM 3 10 1.5 3.1 2.6 1.9 1.5 Quintic 7 2.0 2.5 2.3 1.8 1.4 5.3 Summary 76 These experimental results support the results from Chapter 4. Namely, for high actuator jerk limits, the controller cannot keep the end-effector on the path. Figures 5.2, 5.3, and 5.4 show that trajectories with high jerks result in increased tracking errors, which, in turn, activate the controller, saturating the actuators. Whenever this happens, the end-effector leaves the path. Such a trajectory is an infeasible trajectory. For the case of the SCORBOT ER VII manipulator, actuator jerk limits more than an order of magnitude higher than actuator torque limits are not sufficient to ensure that the end-effector follows the planned path. While this result was not predicted by the simulations, it does not, however, present itself as totally unexpected. Due to the large errors involved in modelling the system, one would expect that the simulation results would overestimate the system capabilities. The experimental performance of the SPCTOM trajectory corresponding to the low jerk limits, i.e. lONm/sec, is similar to its simulated performance. Thus, while being tracked by the controller with the same accuracy and the same effort as the quintic trajectory, it results in reduced motion time. This indicates that actuator jerk limits are preferable when determining smooth time optimal motions over global velocity and acceleration limits. The SPCTOM trajectories above suggest that, for the SCORBOT ER VII, realistic jerk limits are approximately equal to the actuator torque limits. To verify this proposition, SPCTOM tra-jectories were determined for jerk limits of 50Nm/sec and 20Nm/sec and the robot was required to follow them. As seen in Figures 5.7 and 5.8, the jerk limits of 50Nm/sec are still too high for the system tracking capabilities. The SPCTOM trajectory corresponding to the 20Nm/sec limits results in lower motion time and slightly decreased tracking performance. 5.3 Summary In this chapter, experimental results obtained when implementing the PCTOM, SPCTOM cor-responding to different jerk limits, and quintic trajectories were presented. These results confirmed that the actuator jerk limits are instrumental in determining feasible time optimal trajectories when an accurate dynamic model of the system is not available. They also demonstrated that trajectory smoothness is directly related to the tracking performance of the controller. No specific rules were obtained for how to choose appropriate jerk limits. However, the ex-5.3 Summary 77 Axis 1 - Control Performance A x ' s ^ ~ Control Performance Axis 3 - Control Performance time [sec] time [sec] time [sec] (a) Joint 1 performance (b) Joint 2 performance (c) Joint 3 performance Figure 5.7: Experimental results for the SPCTOM trajectory (jerk limits of 20Nm/sec) implemented on the SCORBOT ER VII. Axis 1 - Control Performance A x i s ^ - Control Performance Axis 3 - Control Performance time [sec] time [sec] time [sec] (a) Joint 1 performance (b) Joint 2 performance (c) Joint 3 performance Figure 5.8: Experimental results for the SPCTOM trajectory (jerk limits of 50Nm/sec) implemented on the SCORBOT ER VII. periments have shown that these limits should be fairly conservative when applied to inaccurately modelled systems. Thus, actuator jerk limits can compensate for large model errors. The trade-off is an increase in motion time. Chapter 6 Conclusions and Recommendations 6.1 Summary In this thesis, a strategy for planning SPCTOM trajectories has been presented. These time optimal trajectories are determined by imposing actuator jerk limits on the proposed motion. Such trajectories can be directly implemented on an industrial robot when the dynamic parameters are not accurately known, without the need for a sophisticated controller. They represent, therefore, feasible time optimal motions. The SPCTOM trajectories provide a practical compromise between the need for shorter manip-ulation task completion times and the need for increased tracking accuracy and decreased vibrations of the robot system. Previous planning strategies have incorporated the smoothness requirement as smoothness of the joint space trajectory and have imposed limits on the joint velocities, accelerations, and jerks or on the end-effector velocity and acceleration. In this thesis, the trajectory smoothness is defined in the phase space and limits on the actuator jerks are used in the planning approach. The solution proposed for determining SPCTOM trajectories consists in transforming the TOC problem into a nonlinear parameter optimization problem and solving it using the F T M in [27]. The adequacy of this optimization technique is shown by setting the jerk limits very high and showing that the SPCTOM trajectory converges towards the P C T O M trajectory, within the limits 78 6.2 Recommendations 79 of the chosen trajectory parameterization. While the negative effect of lowering the actuator jerk limits on the motion time is visible from the planning stage, the influence of the jerk limits on the trajectory feasibility is studied through simulations and experiments. Both demonstrate that a trajectory which requires high actuator jerks is not a feasible trajectory for a poorly modelled system controlled by a simple PID independent joint controller. SPCTOM trajectories, however, represent feasible time optimal motions and therefore they have industrial relevance. Moreover, the limits on the actuator jerks are related to the tracking performance of the controller and the control effort of the actuators. The simulations and the experiments are also conducted with the goal of comparing the SPCTOM trajectory to the smooth quintic trajectories. The results are favourable to the SPCTOM trajec-tory: for similar actuator jerks and controller effort for both trajectories, the SPCTOM trajectory is characterized by a significantly shorter motion time. This result shows that SPCTOM trajecto-ries with fixed actuator jerk limits provide a superior method of defining optimal smooth motions over quintics with fixed velocity and acceleration limits. This is due to the fact that velocity and acceleration limits are position dependent and, therefore, are determined for the worst case sce-nario. The actuator jerk limits, on the other hand, are position independent. Thus, they represent a uniform measure for the trajectory smoothness over the entire robot workspace. The disadvantage of planning a SPCTOM trajectory over planning a quintic trajectory is the required computational effort. While an optimal quintic involves practically no computational load, a SPCTOM is planned through a nonlinear optimization and requires considerable computational effort. Therefore, at this stage, the SPCTOM trajectory can only be used when the planning is done off-line. 6.2 Recommendations In this thesis, SPCTOM trajectories with limited actuator jerks were proven to represent feasible trajectories even for a system with roughly estimated dynamic parameters controlled by an unso-phisticated PID controller, being therefore readily implementable in any robotic system. Moreover, they compared favourably to the optimal quintic trajectories. However, several research directions which could improve the present work immediately suggest themselves. They are presented herein. 6.2 Recommendations 80 (1) At the theoretical level, one could investigate the optimality conditions for the SPCTOM. This could potentially lead to the derivation of an algorithm for computing SPCTOM similar to that presented in [5] for computing the PCTOM. The advantage of such an efficient search would be its much lower computational requirements, which would make SPCTOM trajectories suitable for on-line planning. This would considerably increase the practical significance of such trajectories, since they could be used in planning any path-constrained manipulation task. (2) At the experimental level, one could investigate the selection of the actuator jerk limits. The research reported here indicates that they should be in the same range as the actuator torque limits. However, more work is required to determine whether they are only actuator dependent or they are also influenced by factors like model inaccuracies or friction. All these are reasonable assumptions and they need to be verified by experimental research. (3) Finally, one could investigate how jerk limits can be directly related to robot vibrations and wear. This correlation might be difficult to establish, since it entails developing a reasonably easy to use measure for vibrations and their influence on robot wear. Furthermore, it could require longitudinal studies with access to working industrial robots. Nevertheless, it would provide an objective basis for determining what the optimal task productivity should be when both short term and long term consequences would be considered. The research directions outlined here would be challenging and they would contribute to defining task time optimality in a more realistic way, with a more careful consideration for the physical limitations of a robotic system. To various degree, they would also contribute to the OCT, as well as to the problem of on-line time-optimal trajectory planning. Bibliography S.K. Agrawal. Inertia Matrix Singularity of Spatial Series-Chain Manipulators with Point Masses. ASME Transactions, Journal of Dynamic Systems, Measurement, and Control, 31:131-136, 1991. R.L. Andersson. A Robot Ping-Pong Player: Experiment in Real-Time Intelligent Control. MIT Press, Cambridge, MA, 1988. Robotic Industries Association. ANSI/RIA R15.06-1992 Standard, 1992. J.E. Bobrow. Optimal Robot Path Planning using Minimum-Time Criterion. IEEE Transac-tions on Robotics and Automation, 4:443-450, 1988. J.E. Bobrow, S. Dubowsky, and J.S. Gibson. Time-Optimal Control of Robotic Manipulators Along Specified Paths. The International Journal of Robotics Research, 4(3):3-17, 1985. H.G. Bock and K.-J . Plitt. A Multiple Shooting Algorithm for Direct Solution of Optimal Control Problems. In 9th IFAC World Congress, pages 1853-1858, Budapest, Hungary, 1984. J. Butler and M. Tomizuka. A Suboptimal Reference Generation Technique for Robotic Ma-nipulators Following Specified Paths. Robotics Research, 14:37-42, 1989. B. Cao, G.I. Dodds, and G.W. Irwin. Time-Optimal and Smooth Constrained Path Planning for Robot Manipulators. In IEEE International Conference on Robotics and Automation, pages 1853-1858, San Diego, California, 1994. Y. -H. Chang, T.-T. Lee, and C.-H. Liu. On-Line Cartesian Path Trajectory Planning for Robot Manipulators. In IEEE International Conference on Robotics and Automation, pages 62-67, 1988. Y. -H. Chang, T.-T. Lee, and C.-H. Liu. On-Line Approximate Cartesian Path Trajectory Planning for Robotic Manipulators. IEEE Transactions on Systems, Man, and Cybernetics, 22(3):542-547, May/June 1992. Y. Chen and A. A. Desrochers. Structure of Minimum-Time Control Law for Robotic Manipula-tors with Constrained Paths. In IEEE International Conference on Robotics and Automation, pages 971-976, Piscataway, New Jersey, 1989. Y . Chen and A.A. Desrochers. A Proof of the Structure of the Minimum-Time Control Law of Robotic Manipulators using a Hamiltonian Formulation. IEEE Transactions on Robotics and Automation, 6(3):388-393, June 1990. 81 BIBLIOGRAPHY 82 Y. Chen and J. Huang. An Improved Computation of Time-Optimal Control Trajectory for Robotic Point-To-Point Motion. International Journal of Control, 65(5):177-194, 1996. Y. Chen, J. Huang, and T.Y. Wen. A Continuation Method for Time-Optimal Control Syn-thesis for Robotic Point-To-Point Motion. In 32nd Conference on Decision and Control, pages 1628-1632, San Antonio, Texas, December 1993. W.L. Cleghorn, J.F. Fu, and R.G. Fenton. A General Method for Optimum Design of Gear Boxes through Nonlinear Programming. In ASME Design and Automation Conference, volume DE-2, pages 153-160, Montreal, Quebec, 1989. E.A. Croft, B. Benhabib, and R.G. Fenton. Near-Time Optimal Robot Motion Planning for On-line Applications. Journal of Robotic Systems, 12(8):553-567, 1995. O. Dahl and L. Nielsen. Torque-Limited Path Following by On-Line Trajectory Time Scaling. IEEE Transactions on Robotics and Automation, 6(5):554-561, 1990. Carl de Boor. A Practical Guide to Splines. Springer-Verlag, New York, 1978. B.R. Donald and P.G. Xavier. Time-Safety Tradeoffs and a Bang-Bang Algorithm for Kinody-namic Planning. In IEEE International Conference on Intelligent Robots and Systems, pages 552-557, Sacramento, California, April 1991. S. Dubowsky, M.A. Norris, and Z. Shiller. Time Optimal Trajectory Planning for Robotic Manipulators. In IEEE International Conference on Robotics and Automation, pages 1906— 1912, San Francisco, California, April 1986. N.A. Erol and Y . Altintas. Open Architecture Modular Tool Kit for Motion and Process Control. In ASME International Mechanical Engineering Congress and Exposition, ASME Publication MED, pages 15-22, Dallas, Texas, 1997. R.G. Fenton and J. Xu. Kinetic Synthesis of Mechanisms Using the Flexible Tolerance Algo-rithm. In Proceedings of the 7th World Congress of the Theory of Machines and Mechanisms, volume 1, pages 37-40, Sevilla, Spain, 1987. J.Y. Fourquet. Optimal Control Theory and Complexity of the Time-Optimal Problem for Rigid Manipulators. In IEEE International Conference on Intelligent Robots and Systems, pages 84-90, Yokohama, Japan, July 1993. H.P. Geering, L. Guzzella, S.A.R. Hepner, and O H . Onder. Time-Optimal Motions of Robots in Assembly Tasks. IEEE Transactions on Automatic Control, AC-31(6):512-518, 1986. E.G. Gilbert and D.W. Johnson. Distance Functions and their Applications to Robot Path Planning in the Presence of Obstacles. IEEE Journal of Robotics and Automation, RA-L21-30, 1985. G. Heinzinger, P. Jacobs, J. Canny, and B. Paden. Time-Optimal Trajectories for a Robot Manipulator: A Provably Good Approximation Algorithm. In IEEE International Conference on Robotics and Automation, pages 150-156, Cincinnati, Ohio, May 1990. D.M. Himmelblau. Applied Nonlinear Programming. McGraw-Hill, 1989. BIBLIOGRAPHY 83 [28] M Hoffman, A. Malowany, and J. Angeles. Near-Minimum-Time Trajectories for Pick-and-Place Operations. In ASME International Conference on Computers in Engineering, pages 433-438, San Francisco, California, August 1988. [29] J.M. Hollerbach. Dynamic Scaling of Manipulator Trajectories. ASME Transactions, Journal of Dynamic Systems, Measurement, and Control, 106:102-106, 1984. [30] D.W. Johnson and E.G. Gilbert. Minimum-Time Robot Path Planning in the Presence of Obstacles. In 24th Conference on Decison and Control, pages 1748-1753, Ft. Lauderdale, Florida, December 1985. [31] M.E. Kahn and B. Roth. The Near-Minimum Time Control of Open-Loop Articulated Kine-matic Chains. ASME Transactions, Journal of Dynamic Systems, Measurement, and Control, pages 164-172, 1971. [32] J.C. Kieffer, A.J . Cahill, and M.R. James. Robust and Accurate Time-Optimal Path-Tracking Control for Robot Manipulators. IEEE Transactions on Robotics and Automation, 13:880-889, December 1997. [33] K. Kyriakopoulos and G. Saridis. Minimum Jerk Path Generation. In IEEE International Conference on Robotics and Automation, pages 364-369, Philadelphia, Pennsylvania, April 1988. [34] K. Kyriakopoulos and G. Saridis. Minimum Jerk for Trajectory Planning and Control. Robot-ica, 12:109-113, 1994. [35] G. Leitman. The Calculus of Variations and Optimal Control. Plenum Press-New York and London,1981. [36] F.L. Lewis, C T . Abdallah, and D.M. Dawson. Control of Robot Manipulators. Macmillan Publishing Company, New York, 1993. [37] J. L i , R.W. Longman, V.H. Schultz, and H.G. Bock. The Choice of Appropriate Cost Func-tionals for Optimizing the Operating Speed of Robots. In AAS/AIAA Astrodynamics, pages 1821-1840, Sun Valley, Idaho, 1997. [38] C.S. Lin, P.R. Chang, and J.Y.S. Luh. Formulation and Optimization of Cubic Polynomial Joint Trajectories for Industrial Robots. IEEE Transactions on Automatic Control, AC-28(12):1066-1074, 1983. [39] J.Y.S. Luh and C.S. Lin. Approximate Joint Trajectories for Control of Industrial Robots along Cartesian Paths. IEEE Transactions on Systems, Man, and Cybernetics, SMC-14(3):444-450, May/June 1984. [40] B.J. Martin and J.E. Bobrow. Determination of Minimum -Effort Motions for General Open Chains. In IEEE International Conference on Robotics and Automation, pages 1160-1165, Piscataway, New Jersey, 1995. [41] J.M. McCarthy and J.E. Bobrow. The Number of Saturated Actuators and Constraint Forces-During Time-Optimal Movement of a General Robotic System. In IEEE International Con-ference on Robotics and Automation, pages 542-546, Nice, France, 1992. BIBLIOGRAPHY 84 J.A. Nelder and R. Mead. A Simplex Method for Function Minimization. Computer Journal, 4:308-313, 1964. W.S. Newman. Robust Near-Time Optimal Control. IEEE Transactions on Automatic Control, 35(7):841-844, 1990. D. Paviani and D.M. Himmelblau. Constrained Nonlinear Optimization by Heuristic Program-ming. Operations Research, 17:872-882, 1969. F. Pfeiffer and R. Johanni. A Concept for Manipulator Trajectory Planning. IEEE Interna-tional Journal of Robotics and Automation, RA-3(2):115-123, 1987. V.T. Rajan. Minimum-Time Trajectory Planning. In IEEE International Conference on Robotics and Automation, pages 759-764, St. Louis, Montana, 1985. G. Sahar and J.M. Hollerbach. Planning of Minimum-Time Trajectories for Robot Arms. In IEEE International Conference on Robotics and Automation, pages 751-758, St. Louis, Montana, 1985. Z. Shiller. Interactive Time-Optimal Robot Motion Planning and Work-Cell Layout Design. In IEEE International Conference on Robotics and Automation, pages 964-969, Phoenix, Ari-zona, May 1989. Z. Shiller. On Singular Points and Arcs in Path Constrained Time Optimal Motions. Advances in Robotics, ASME Winter Annual Meeting, Anaheim, California, DSC-42:141-147, 1992. Z. Shiller. Time-Energy Optimal Control of Articulated Systems with Geometric Path Con-straints. In IEEE International Conference on Robotics and Automation, pages 2680-2685, San Diego, California, 1994. Z. Shiller, H. Chang, and V. Wong. The Practical Implementation of Time-Optimal Control for Robotic Manipulators. Robotics and Computer-Integrated Manufacturing, 12(1):29—39, 1996. Z. Shiller and S. Dubowsky. The Acceleration Map and Its Use in Minimum Time Motion Planning of Robotic Manipulators. In ASME International Conference on Computers in En-gineering, pages 229-234, New York, New York, August 1987. Z. Shiller and S. Dubowsky. Robot Path Planning with Obstacles, Actuator, Gripper, and Payload Constraints. The International Journal of Robotics Research, 8(6):3—18, 1989. Z. Shiller and H.H. Lu. Computation of Path Constrained Time Optimal Motions with Dy-namic Singularities. ASME Transactions, Journal of Dynamic Systems, Measurament, and , Control, 114(l):34-40, March 1992. K.G. Shin and N.D. McKay. Minimum-Time Control of Robotic Manipulators with Geometric Path Constraints. IEEE Transactions on Automatic Control, AC-30(6):531-541, June 1985. K.G. Shin and N.D. McKay. A Dynamic Programming Aproach to Trajectory Planning of Robotic Manipulators. IEEE Transactions on Automatic Control, AC-31(6):491-500, June 1986. BIBLIOGRAPHY 85 [57] D. Simon. The Application of Neural Networks to Optimal Robot Trajectory Planning. Ro-botics and Autonomous Systems, 11:23-34, 1993. [58] S. Singh and M.C. Leu. Optimal Trajectory Generation for Robotic Manipulators using Dy-namic Programming. ASME Transactions, Journal of Dynamic Systems, Measurament, and Control, 109:88-96, June 1987. [59] J. J.E. Slotine and H.S. Yang. Improving the Efficiency of Time-Optimal Path-Following Al -gorithms. IEEE Transactions on Robotics and Automation, 5(1):118-124, February 1989. [60] E.D. Sontag and H.J. Sussmann. Remarks on the Time-Optimal Control of Two-Links Ma-nipulators. In 24th Conference on Decision and Control, pages 1646-1652, Fort Lauderdale, Florida, December 1985. [61] E.D. Sontag and H.J. Sussmann. Time-Optimal Control of Manipulators. In IEEE Interna-tional Conference on Robotics and Automation, pages 1692-1697, San Francisco, California, April 1986. [62] H.Y. Tam. Minimum Time Closed-Loop Tracking of a Specified Path by Robot. In 29th Conference on Decision and Control, pages 971-976, Honolulu, Hawaii, December 1990. [63] M. Tarkiainen and Z. Shiller. Time Optimal Motions of Manipulators with Actuator Dynam-ics. In IEEE International Conference on Robotics and Automation, pages 725-730, Atlanta, Georgia, 1993. [64] M. Vukobratovic and M . Kircanski. A Method for Optimal Synthesis of Manipulator Robot Trajectories. ASME Transactions, Journal of Dynamic Systems, Measurament, and Control, 104:188-193, June 1982. [65] D. Wang and Y. Hamam. Optimal Trajectory Planning of Manipulators with Collision Detec-tion and Avoidance. The International Journal of Robotics Research, ll(5):460-468, October 1992. [66] H. Wappenhans, J. Holzl, J Steinle, and F. Pfeiffer. Optimal Trajectory Planning with Ap-plication to Industrial Robots. International Journal of Advanced Manufacturing Technology, 9:49-55, 1994. Appendix A The Flexible Tolerance Method The Flexible Tolerance Method F T M was introduced by Paviani and Himmelblau [44]. The detailed description of the algorithm and its concepts is given in [27]. Herein, an overview of the method is presented along with the details of its implementation for the SPCTOM trajectory planning problem. In this appendix, the symbols used within the main body of the thesis are overloaded. This is done in order to maintain the common mathematical notation. Al l symbols are redefined as they are used, but the definitions apply only to this appendix. A . l Overview of the F T M Using the F T M , the general optimization problem is: Minimize: /(x) x e Rn (A-l) Subject to: /ij(x) = 0 i = 1, (A-2) 5i(x) > 0 i = l + l,...,m, (A-3) where /(x) is the objective function, hi(x) are the equality constraints, gi(x) are the inequality constraints, and Rn is the n-dimensional space where x lies. This problem is reformulated into the 86 A.l Overview of the FTM 87 following, simpler but equivalent problem with only one constraint: min: /(x) x e Rn (A-4) subject to: - T(x) > 0, (A-5) where <&(fc) is the value of the flexible tolerance criterion at the fcth stage of the optimization. This criterion also serves as the stopping condition for the search. The cost function /(x) and the equality and inequality constraints in Equations (A-2) and (A-3) may be linear and/or non-linear functions of the variables in x. The basic strategy of the method is to improve the value of the cost function by using information provided by feasible points, as well as certain nonfeasible points called near-feasible points. The limits on the near-feasibility are made more restrictive as the search advances until, in the limit, only feasible points are accepted. As a result of this strategy, the algorithm does not spend a considerable portion of computation time on satisfying rigorous feasibility constraints and its computational efficiency is greatly improved. In (A-5), T(x) is a positive functional of all the equality and/or inequality constraints of the original problem and it is used as a measure of the constraint violation, while <f> is selected as a positive decreasing function of the x points in Rn. Thus, <E> serves as both a limit on the constraint violation throughout the search and as a criterion for the termination of the search. The tolerance criterion is used to categorize points in Rn. At the A;-th stage of the optimization, a point x(fc) is said to be: 1. Feasible, if T(x) = 0 2. Near-feasible, if 0 < T(x) < 3. Nonfeasible, if T(x) > A small value of T(x(fc)) implies that x(fe) is relatively near to the feasible region, and a large value of T(x(fe)) implies that x(fc) is relatively far from the feasible region. The region of near-feasibility is therefore defined by: $ W - T ( x ) > 0 . (A-6) On a transition from x ^ to x.(k+1\ the move is said to be feasible if 0 < T(x( f c + 1)) < and A.2 Overview of the FPM 88 nonfeasible if < T( X ( f c + 1 )) . The flexible polyhedron method (FPM) of Nelder and Mead [42] is used to minimize /(x) when the constraint (A-5) is not active. The general idea is to reduce <f>(fc) as the search progresses, thus tightening the near-feasibility region, and to segregate the minimization of /(x) from the steps taken to satisfy the constraint (A-5). For a given value of cp(fc); the value of T(x) at x(fc+1) will be: • either T(x(fc+1)) < in which case x(fc+1) is either a feasible or near-feasible point and the move is accepted as a permitted move; • or T(x(fc+1)) > in which case x(fc+1) is nonfeasible and a point x in the feasible or near-feasible region must be found in lieu of x(fc+1). It is possible to use the same minimization technique for replacing an infeasible x(fc+1) with a feasible point, but any algorithm that achieves this purpose is sufficient. In summary, the F T M is a nonlinear constraint optimization technique in which all equality and inequality constraints are transformed into one single inequality constraint. This constraint defines the near-feasibility limit at each stage of the optimization. The algorithm improves the value of the cost function using the F P M as the "outer" optimization when the constraint is not active. When the F P M generates an infeasible point (the constraint becomes active), the F T M algorithm uses another "inner" optimization to replace it with a feasible or near-feasible point. The strategy is to progressively tighten the near-feasibility region and to separate the two optimizations. A.2 Overview of the F P M The Flexible Polyhedron Method, F P M [42], is briefly presented here since it is used in the implementation of the F T M , as the "outer", unconstrained optimization. The F P M is an unconstrained multi-dimensional minimization which requires only function evaluations. In advancing the search, the method uses information about the value of the objective function at some points called the vertices of the polyhedron. In an n-dimensional search space, information about n + 1 points is needed. The polyhedron is a geometrical figure consisting of n + 1 points and all their interconnecting line segments, polygonal faces, etc. At any stage of the optimization; the polyhedron encloses a finite n-dimensional volume. A.3 Implementation Details 89 The algorithm replaces the point with the largest value of the objective function ("highest" point) with a lower point. One stage of the search consists of the following steps: • The highest point is reflected through the centroid of the remaining n points and the reflection point is kept if it is higher than the lowest point but lower than at least two other points. • If the reflection point is lower than the lowest point, it is expanded in the same direction. The expansion point is kept if it is lower than the lowest point. Otherwise, the reflection point is kept. • If the reflection failed, the highest point is contracted toward the centroid and the contraction point is kept if it is lower than the highest point. • If contraction failed, all points are pulled around the lowest (best) point. Thus, the polyhedron tends to conserve its volume, but it might expand when a good search direction is found, or it might contract when it reaches a "valley floor". The search is initialized by providing it with n + 1 points and it is stopped when the decrease in the objective function value is smaller than some predetermined tolerance. A.3 Implementation Details The details of the F T M implementation for computing SPCTOM trajectories are presented herein. Al l the references will be made to the elbow manipulator in Table 2.1 along the parabolic path (2.1) and figures related to this example will be used to illustrate the notation. As explained in Chapter 3, the vector of optimization variables is: As shown in Figure 3.3, for the chosen example, p = 5, i.e., there are 5 switching points in the PCTOM, and the dimension of the space to be searched is n = 7. controlled and is kept constant over each step. Thus, if two consecutive points along the trajectory are considered, then the time taken by the manipulator to move from Sj, S{, s\ to Si+i, Sj+i, s'i+i (A-7) The objective function /(x) is the motion time. It is computed assuming that the pseudo-jerk is A.3 Implementation Details 90 is determined by solving the system: 's'i • t2 si+i = Si + Si-t-\ — (A-8) Si+l = 5i + S i - t + - y - + , (A-9) where t and 's'i are considered unknown. The state is then updated according to: s'i+i = Si + 's'i • t. (A-10) The inequality constraints (A-3) for the problem of SPCTOM are defined as: T £ 4 ( i - i ) + i ( x ) = 1 -max — — , ( A - l l ) s(s) J-max,i T ff4(i-i)+2(x) = 1 -max— l—, (A-12) Lmin,i T 54(i_1 ) + 3(x) = 1 - max ' , (A-13) s(s) l m a X t i T 94(i-i)+4(x) = l - m a x ^ — , (A-14) s\s) ->-min,i where i takes values from 1 to dof, the number of degrees of freedom of the manipulator. This definition ensures that whenever any of the joint torques and/or joint jerks exceeds its limits, the respective constraint becomes negative. For the case presented in Section 3.3.2, Figure 3.4, the robot is an elbow manipulator and only the positional degrees of freedom are considered; therefore dof= 3. The inequality constraints <7i(x) > 0,i = 1, . . . ,4- dof, with #i(x) defined by Equations (A-11)-(A-14), represent a direct enforcement of the actuator torque and jerk constraints (2.38) and (2.32). The actuator constraints are used directly instead of the state constraints (2.63)-(2.64) and the control constraints (2.65) to simplify the computation. T(x) is a positive functional of all the equality and/or inequality constraints. It is defined as: T(x) = maxj(l - <7i(x)) if 3i such that gi(x) > 1 (A-15) 0 otherwise. A.3 Implementation Details 91 This definition differs from the one in [27]. In [27], T(x) is defined as: 1/2 T(x) = J>?(x)+ £ ^ 2 ( x ) .i=l i=l+l (A-16) where is the Heaviside operator such that Ui = 0 for <7J(X) > 0 and ZYj = 1 for pj(x) < 0. Therefore, if £ j = 1 / i ? ( x ) is convex and the gi(x),i = I + 1... , m, are concave functions, then T(x) is a convex function with a global minimum at T(x) = 0. Since for the SPCTOM problem the inequality constraints are not analytic functions, but can only be computed numerically, no information is available about their convexity. Then, the advantage of Equation (A-16) as the definition of T(x) is lost. Equation (A-15), on the other hand, is easier to compute and interpret. In the implementation presented here, the tolerance criterion <J> is selected to be: n+l *(*> = m i n f ^ - 1 ) ; ^ \\xf> - ^ L l l l (A-17) i=l (k) with k a constant that is appropriately chosen, and xcentr the centroid of the polyhedron after the elimination of the point with the highest value of the cost. The initial value of the tolerance criterion, <&°, is set to 0.2. Thus, the tolerance criterion is a positive non-increasing function of the sequence of points x(°), x^1),... , x(fc),... , x* generated during the progression of the search. Unlike the implementation of the F T M in [27] (where the F P M is used for both the outer and the inner optimizations), the strategy for the inner optimization used herein (that is, for replacing an infeasible point by a feasible or near-feasible one) is a simple line search through bisection. The choice is justified by the high computational cost involved in the motion time and the constraint evaluations. Depending on which point became infeasible, the line search is performed as follows: • If a reflection yielded the infeasible point, the line from the infeasible vertex toward the one from which it resulted is searched. The search stops before the centroid (in order to preserve search space dimension). This situation is schematically depicted in Figure A. 1(a) for a polyhedron with three vertices. • If an extension yielded the infeasible point, the line from the infeasible vertex toward the reflected one is performed. The search stops at the reflected point (Figure A. 1(b)). • If a contraction yielded the infeasible point, the line from the infeasible vertex toward the one A.3 Implementation Details 92 from which it resulted is performed (Figure A.1(c)). • If more vertices became infeasible as the result of a collapse or a change in the tolerance crite-rion, each infeasible vertex is moved toward the origin. As vertices closer to origin represent slower motions, this strategy is guaranteed to find feasible points. However, the move is not performed through bisection, but in small steps with predetermined length (Figure A. 1(d)). (c) Infeasible contrac- (d) Infeasible vertex due tion. to a change in the toler-ance. Figure A . l : The line searches during the replacement of the different types of infeasible points generated by the F P M . The algorithm of the F T M as implemented for the SPCTOM problem is presented in Figures A.2 and A.3. The terminology is the one used in the F P M of Nelder and Mead [42]. A.3 Implementation Details 93 ( Start) Choose tolerance, initial points (vertices). Set k=0. Move infeasible points toward origin Move reflection point toward highest point Replace highest point with reflection point Figure A.2: Flow diagram of the F T M for the SPCTOM problem - Part 1. A.3 Implementation Details 94 Move contraction point toward the highest point Collapse polyhedron Yes Yes Replace highest point with contraction point Replace highest point with expansion point No Replace highest point with reflection point k=k+1 Update the tolerance Figure A.3: Flow diagram of the F T M for the SPCTOM problem - Part 2. Appendix B Robot Dynamic Models In the simulations presented in Chapter 4, two manipulators are used: (i) the elbow manipulator in [45] and (ii) the SCORBOT ER VII in the IAL. The dynamic models of these robots are detailed in this appendix. As shown in Chapter 2, the manipulator Lagrangian dynamics is given by: M(q)-q + qT-C(q)-q+G(q) = T, (B-l) where q G R n is the vector of joint positions, T G R n is the vector of actuator torques, M(q) G R n x n is the inertia matrix of the manipulator, C(q) G R n x n x n is a third order tensor representing the coefficients of the centrifugal and Coriolis forces, G(q) G Hn is the vector of gravity terms, and ' denotes the derivative with respect to time. The third order dynamics, needed in order to incorporate the actuator jerks, is derived from Equation (B-l) by differentiation with respect to time: M(q) • q + M(q) • q + q T • C(q) q + q T C(q) q + q T • C(q) '• q + G(q) = T. (B-2) Equations (B-l) and (B-2) show that the robot model is known if the inertia matrix, the Christof-fel symbols, and the gravity terms are known. Al l the other terms involved in Equations (B-l)-(B-2) are derived by differentiation with respect to time. 95 B.l Elbow Manipulator Dynamic Model 96 B . l E l b o w M a n i p u l a t o r D y n a m i c M o d e l The dynamic parameters of the elbow manipulator in [45] are given in Table 4.1. Using them, the inertia matrix, the Christoffel symbols, and the gravity terms are computed and given hereafter. The non-zero components of the inertia matrix, M(q), are: M n = 1.89c,3 + 8.4455 + 2.4295c2(?2 + 1.89c2q2+q3 + 0.89602^+293 ( B ' 3 ) M22 = 3.78^3 + 6.7771 (B-4) M 2 3 = 1.89c,3 + L 8 1 2 (B-5) M 3 2 = 1.890,3 + L 8 1 2 ( B " 6 ) M 3 3 = 1-812, (B-7) where cq is a notation for cos(q). The Christoffel symbols are computed from the inertia matrix, according to: C * ( ^ + (B-8) 2 V % dqj dql J Performing all the calculations, the non-zero Christoffel symbols are obtained as: Cn2 = -1.89*292+93 - 0.896s29 2+ 2 ( 73 - 2.4295s2(?2 (B-9) C113 = - 0 . 9 4 5 5 2 ( ? 2 + 9 3 - 0.89652^+293 - 0.945a93 (B-10) Cm = - 1 . 89s2 9 2 + 9 3 - 0.896s 2 g 2+29 3 - 2.4295s2g2 (B-ll) Cm = -0 .945a 2 9 2 + M - 0.896s 2 g 2+ 29 3 - 0.945s93 (B-12) C211 = 1.89s 2 9 2 +,3 + 0.896s292+293 + 2.429552g2 (B-13) C 2 2 3 = -1.89s,3 (B-14) C 2 3 2 = -1.89s,3 (B-15) C 2 3 3 = -1.89s93 ( B " 1 6 ) B.2 SCORBOT ER VII Dynamic Model 97 C 3 n = 0 . 9 4 5 s 2 g 2 + 9 3 + 0 .8965292+293 + 0 . 9 4 5 s 9 3 (B-17) C322 = -1.89s g s, (B-18) where sq is a notation for sin(q). Finally, the non-zero components of the vector of gravity terms are: G2 = 66.5118cg2 + 24.7212C(?2+(?3 (B-19) G 3 = 24.7212cg 2 + g 3. (B-20) B.2 S C O R B O T E R VII Dynamic Model The DH and dynamic parameters of the first three links (positional dof) of the SCORBOT ER VII in the IAL are given in Tables 4.8 and 4.9, respectively. Using them, after performing all the required calculations, the elements of the inertia matrix of the SCORBOT ER VII are: M n = 0.1575c292+(?3 + 0.033c92 + 0.1575c93 + 0.0478c2(?2+293 + 0.27725c2(?2 + 0.05250,2+53 + 0.9499 (B-21) M 1 2 = -0.019s 9 2 + 9 3 - 0.012sg2 (B-22) M13 = -0.019 5 < ? 2 + g 3 (B-23) M 2 i = -0.019s9 2+9 3 - 0.012sg2 (B-24) M 2 2 = 0.315cq3 + 0.94 (B-25) M 2 3 = 0.1575C(?3 + 0.1656 (B-26) M31 = -0.019sg 2+g 3 (B-27) M 3 2 = 0.1575c93 + 0.1656 (B-28) M 3 3 = 0.1656, (B-29) B.2 SCORBOT ER VII Dynamic Model 98 the non-zero elements of the Christoffel symbols are: Cn2 = -0.1575s 2 ( ? 2 + ( ? 3 - 0.0478s2(?2+2O3 - 0.01655(?2 - 0.02625sM + 9 3 - 0.27725s2(?2 (B-30) Cm = -0.07875s2g2+ (,3 - 0.0478s 2 g 2 + 2 „ 3 - 0.07875s„3 - 0.02625sq2+(,3 (B-31) Cl2l = -0.1575s 2 g 2 + 9 3 - 0.0478S292+293 - 0.0165s92 - 0.02625sg 2 + g 3 - 0.27725s2(?2 (B-32) C 1 2 2 = -0.012C ( ? 2 - 0.019c92+<73 (B-33) C i 2 3 = -0.019 C ( ? 2 + ( ? 3 (B-34) C13l = -0.07875s 2 ( ? 2 + ( ? 3 - 0.0478s 2 g 2 + 2 9 3 - 0.07875s93 - 0.02625sg2+(?3 (B-35) C 1 3 2 = -0.019c g 2 + ( ? 3 (B-36) C133 = -0.019c g 2 + g 3 (B-37) C2ll = 0.1575s 2 g 2 + g 3 + 0.0478s 2 ( ? 2 + 2 g 3 + 0.0165sg2 + 0.026255 ( ? 2 + ( ? 3 + 0.27725s2(?2 (B-38) C 2 2 3 = -0.1575s93 (B-39) C232 = -0.1575^3 (B-40) C 2 3 3 = -0.1575sg3 (B-41) C311 = 0.07875s2g.2+93 + 0.0478s 2 ( ? 2 + 2 9 3 + 0.07875sg3 + 0.02625s9 2 + g 3 (B-42) C 3 2 2 = 0.1575sg3, (B-43) and the non-zero components of the gravity vector result as: G2 = -3.2373c92 - 5.15c g 2 + g 3 (B-44) G 3 = -5.15c g 2 + 9 3 . (B-45)
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- Smooth time optimal trajectory planning for industrial...
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
Smooth time optimal trajectory planning for industrial manipulators Constantinescu, Daniela 1998
pdf
Page Metadata
Item Metadata
Title | Smooth time optimal trajectory planning for industrial manipulators |
Creator |
Constantinescu, Daniela |
Date Issued | 1998 |
Description | This work proposes and demonstrates a strategy for planning smooth path-constrained time-optimal trajectories for manipulators. Such trajectories are obtained by limiting the actuator jerks required by the planned motion. Existing planning strategies incorporate the smoothness requirement either as smoothness of the actuator torques or as smoothness of the joint trajectories. The smoothness requirement is desirable for reducing strain on robot actuators while still requiring low cycle times. In this work, the trajectory smoothness is defined in the phase plane and the planning observes the limits on the actuator jerks. The solution proposed for determining the optimal trajectories consists of approximating the time optimal control problem by a nonlinear parameter optimization problem which is solved using the flexible tolerance method. It is shown that the approximate solution converges to the time optimal motion when the actuator jerks become very high. A number of simulations are performed to demonstrate the proposed strategy. These simulations show that actuator jerk limits have a negative impact on robot motion time, but they do not give any indication about robot trajectory feasibility. This aspect is studied through further simulations and experiments on an industrial robot. The results of this work show that the tracking accuracy is directly related to the actuator jerk limits. Therefore, it is necessary to impose such limits when planning feasible optimal trajectories. Finally, the performance of the smooth time optimal motion is compared to the performance of both the non-jerk limited optimal trajectory, as well as a smooth quintic trajectory. For similar actuator jerks and controller effort, the smooth path-constrained time-optimal trajectory results in a significantly shorter motion time with nearly the same tracking accuracy as a quintic polynomial. Based on the results in this work, actuator jerk limits are shown to provide an improved method of achieving a compromise between high tracking accuracy, smooth joint behaviour, and optimal motion time. |
Extent | 6732301 bytes |
Genre |
Thesis/Dissertation |
Type |
Text |
FileFormat | application/pdf |
Language | eng |
Date Available | 2009-05-21 |
Provider | Vancouver : University of British Columbia Library |
Rights | For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use. |
DOI | 10.14288/1.0088569 |
URI | http://hdl.handle.net/2429/8051 |
Degree |
Master of Applied Science - MASc |
Program |
Mechanical Engineering |
Affiliation |
Applied Science, Faculty of Mechanical Engineering, Department of |
Degree Grantor | University of British Columbia |
GraduationDate | 1998-11 |
Campus |
UBCV |
Scholarly Level | Graduate |
AggregatedSourceRepository | DSpace |
Download
- Media
- 831-ubc_1998-0399.pdf [ 6.42MB ]
- Metadata
- JSON: 831-1.0088569.json
- JSON-LD: 831-1.0088569-ld.json
- RDF/XML (Pretty): 831-1.0088569-rdf.xml
- RDF/JSON: 831-1.0088569-rdf.json
- Turtle: 831-1.0088569-turtle.txt
- N-Triples: 831-1.0088569-rdf-ntriples.txt
- Original Record: 831-1.0088569-source.json
- Full Text
- 831-1.0088569-fulltext.txt
- Citation
- 831-1.0088569.ris
Full Text
Cite
Citation Scheme:
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>
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-0088569/manifest